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Abstract 


In this diploma thesis, a Schunk SDH2 gripper hand is used as an end effector on a 
KUKA LBR iiwa 14 R820 industrial robot. A Basler avA1000-gc industrial camera is 
mounted next to the gripper. The pose of a known object from the individual images 
taken by the camera is determined by optical object recognition. The goal is to grasp the 
optically detected object with the gripper. For this purpose, the dynamic model of the 
robot extended by the gripper and the camera is derived. Additionally, in order to plan 
a grasp and thus the finger motion, further state variables of the grasp are defined. For 
an unambiguous conversion between the gripper and the grasp states, the forward and 
backward kinematics are derived. 

For a successful grasping of the object, the motion of the robot and the fingers are 
planned first in an offline trajectory planning. In order to react to any errors and also 
updates of the pose estimation of the grasping object during the robot motion, a fast 
online trajectory replanning is implemented. 

The planned trajectory of the robot serves for the feedforward controller and a trajectory 
tracking controller accounts for the trajectory errors. Feedforward and feedback control 
are also implemented for the grasping hand. Several experiments are performed and the 
results are presented and discussed. 
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Kurzzusammenfassung 


In dieser Arbeit wird auf einem KUKA LBR iiwa 14 R820 Industrieroboter eine Schunk 
SDH2 Greifhand als Endeffektor montiert. Neben der Greifhand ist eine Basler avA1000-gc 
Industriekamera angebracht. Mittels optischer Objekterkennung wird aus den einzelnen 
Bildern der Kamera von einem bekannten Objekt die Pose bestimmt. Ziel ist es, das 
optisch detektierte Objekt mit der Greifhand zu greifen. Dazu wird zunächst für den um 
die Greifhand und die Kamera erweiterten Roboter das dynamische Modell hergeleitet. Um 
einen Griff und damit die Fingerbewegung zu planen, werden weitere Zustandsgrößen des 
Griffs definiert. Für eine eindeutige Umrechnung zwischen den Zuständen der Greifhand 
und des Griffs werden die Vorwärts- und die Rückwärtskinematik hergeleitet. 

Für ein erfolgreiches Greifen des Objekts wird zunächst in einer Offline-Trajektorienpla- 
nung die Bewegung des Roboters und der Finger geplant. Um während der Roboterbewe- 
gung auf etwaige Fehler bzw. Updates der Posenschätzung des Greifobjekts zu reagieren, 
wird eine schnelle Online-Trajektorienplanung implementiert. 

Die geplante Trajektorie des Roboters wird während des Versuchs vorgesteuert und ein 
Trajektorienfolgeregler regelt die Trajektorienfehler. Für die Greifhand wird ebenfalls eine 
Vorsteuerung und ein Regler implementiert. Es werden mehrere experimentelle Versuche 
durchgeführt und die Ergebnisse dargestellt und diskutiert. 
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1 Motivation und Einleitung 


Durch den steigenden Grad der Automatisierung werden in der Industrie immer mehr 
Roboter eingesetzt. Gleichbleibend hohe Produktionsqualität, sicherere Arbeitsabläufe 
und somit geringeres Verletzungsrisiko, effizientere Produktion und langfristige Senkung 
der Produktionskosten sind die Treiber für den Einsatz von Industrierobotern. Eine 
große Herausforderung für Unternehmen der heutigen Zeit ist die wachsende Anzahl 
an unterschiedlichen Produkten in immer kürzer werdenden Zeitintervallen. Aufgrund 
dessen geht ein Trend in Richtung eines modularen Produktionsaufbaus, in dem sich 
nicht benötigte Komponenten einfach zu- und abschalten lassen. In diesem flexiblen 
Setting spielen Roboter eine wesentliche Rolle, da sie einfach austauschbar sind und 
schnell auf neue Produkte bzw. Produktionsprozesse angepasst werden können. Für 
robotergestützte Anwendungen wird grundsätzlich vorausgesetzt, dass das Werkstück 
immer an der selben Position und der selben Orientierung dem Arbeitsschritt zugeführt 
wird. Dadurch wird der Einsatz dieser Systeme erheblich erleichtert, da sie durch einen 
einfachen Teach-In-Prozess programmiert werden können. Das Zuführen des Werkstücks im 
jeweiligen Arbeitsschritt in zufälliger Position und Orientierung steigert die Komplexität 
und den Programmieraufwand erheblich, erhöht jedoch die Flexibilität der Produktionslinie. 
Zusätzlich kann durch Optimierung des Greifvorgangs bzw. durch dynamisches Greifen die 
Zykluszeit des Produktionsprozesses verringert und somit die Produktionsrate gesteigert 
werden. Die Detektion und die Bestimmung der Pose und die Planung der Roboterbewegung 
kann unter Umständen sehr aufwendig sein. 

Machine Vision hat sich in den vergangenen Jahren durch den Einsatz Künstlicher 
Intelligenz rasant weiterentwickelt. Auf Basis von Convolutional Neural Networks, kurz 
CNNs, ist es mittlerweile möglich Objekte in Bildern zu detektieren und deren Pose in 
Relation zur Kamera zu bestimmen. 

In [1] werden aus einem einzelnen Kamerabild für alle erkannten Objekte die acht 
Punkte der Bounding-Box sowie der Mittelpunkt geschätzt. Aus diesen neun Punkten 
wird anschließend über den EPnP (Efficient Perspective-n-Point) Algorithmus [2] die Pose 
berechnet. 

In [3] werden mit einem Deep-CNN aus einem RGB-Bild zunächst Featuremaps generiert. 
Ein weiteres Netzwerk bestimmt die Bereiche im Bild, in denen Objekte abgebildet sind. 
Mit den Features aus den detektierten Bereichen werden über ein weiteres neuronales 
Netzwerk direkt die Posen der Objekte bestimmt. 

In [4] werden für alle Pixel des Bildes Vektoren generiert, die zu sog. Keypoints am 
Objekt zeigen. Anhand dieser Keypoints wird weiters mittels EPnP-Algorithmus die Pose 
bestimmt. 

Die genannten Methoden bestehen entweder aus sehr großen Netzwerken, oder es werden 
mehrere Stufen durchlaufen, was die Inferenzzeit erhöht und somit eine Echtzeitimple- 
mentierung kaum möglich macht. Eine sehr effiziente Methode wird in [5] vorgestellt. 
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1 Motivation und Einleitung 2 


Dort werden aufbauend auf Standardnetzwerken für Real-Time Object-Detection YOLO 
(you only look once) erneut Keypoints für jedes Objekt detektiert und mittels EPnP- 
Algorithmus die Pose bestimmt. 

Das Trainieren solcher Netzwerke benötigt eine große Anzahl annotierter Trainingsdaten 
in Form von Bildern. Üblicherweise werden die Trainingsdaten mit großem Aufwand 
händisch annotiert. In [6] wird eine Methode vorgestellt, mit welcher diese Trainingsdaten 
künstlich und in relativ kurzer Zeit erstellt werden können. Dabei handelt es sich um eine 
Erweiterung für die Unreal Grafikengine, mit welcher zufällige Bilder mit den gewünschten 
Objekten generiert und annotiert werden. Damit können Trainingsdaten für alle typischen 
neuronalen Netze erstellt werden. Nachteil dieser Methode ist, dass es sich bei den 
Trainingsdaten um künstliche Bilder handelt und etwas von den Lichtverhältnissen und 
Rauscheigenschaften von realen Bildern abweicht. 

Mit diesen Methoden ergeben sich ganz neue Möglichkeiten im Bereich der Robotik. Die 
Anwendungen für die online Bestimmung der Pose mittels RGB-Kameras lassen sich in 
die Teilbereiche eye-in-hand und eye-to-hand unterteilen. Bei den eye-in-hand Systemen, 
solche Systeme werden auch als visual servoing Systeme bezeichnet, befindet sich die 
Kamera üblicherweise am Endeffektor des Roboters, ist somit in Bewegung und liefert eine 
detailliertere Darstellung der Szene. Bei eye-to-hand Systemen ist die Kamera stationär 
montiert und auf den Roboter gerichtet, womit eine wenig präzisere Darstellung, aber 
eine bessere globale Übersicht erreicht wird. Die Kombination beider Systeme samt ihren 
Vorteilen findet auch ihre Anwendungen, was die Komplexität jedoch wesentlich erhöht. 
Die Arbeit [7] beschäftigt sich z.B. mit einem Setup nach dem eye-in-hand Prinzip, bei 
dem Werkteile von Paletten gesammelt werden. Diese Diplomarbeit richtet sich ebenfalls 
nach dem eye-in-hand Prinzip. 

Roboter werden hauptsächlich stationär in die Industrieanlage integriert. Je nach 
Anwendungsfall bzw. Fertigungsprozess, z.B. dem Entnehmen eines fertigen Bauteils aus 
einer CNC-Maschine und einsetzen eines neuen Rohlings, ergeben sich für den Roboter 
erhebliche Wartezeiten. Ein Roboter auf einer mobilen Plattform, der zwischen den 
einzelnen Fertigungsmaschinen wechselt, ist wesentlich effizienter und kostengünstiger im 
Vergleich zu einem Roboter an jeder Fertigungsmaschine. In [8] wird ein Roboter auf 
einer mobilen Plattform vorgestellt. Die Grobpositionierung des Roboters erfolgt über 
die Plattform, mittels Kameras wird eine Feinpositionierung ausgeführt. In [9] wird ein 
weiterer Roboter auf einer mobilen Plattform vorgestellt. 

Eine Herausforderung ist weiterhin die Trajektorienplanung. Dabei wird zwischen Oflline- 
und Online-Trajektorienplanung unterschieden. Die Ofine-Trajektorienplanung findet 
vor der eigentlichen Roboterbewegung, also offline, statt. Im Gegensatz dazu passiert die 
Online-Trajektorienplanung während der Ausführung, also online. Im Vergleich zur Offline- 
Trajektorienplanung sind bei der Online-Trajektorienplanung die Anforderungen an die 
Hardware und der Rechenaufwand wesentlich größer. In [10, 11] werden Methoden für eine 
schnelle Online-Neuberechnung der Trajektorie vorgestellt. In [10] wird für einen Portalkran 
der kontinuierliche Start- und der Zielbereich jeweils in ein Gitter mit äquidistantem 
Abstand zwischen den Gitterpunkten diskretisiert. Von jedem Gitterpunkt im Startbereich 
wird zunächst in einer Offline-Trajektorienplanung eine optimale Trajektorie zu jedem 
Gitterpunkt im Zielbereich berechnet und in einer Datenbank gespeichert. Für Start- 
und Zielkonfigurationen zwischen den Gitterpunkten wird die optimale Trajektorie mit 
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1 Motivation und Einleitung 1 Motivation und Einleitung 3 


dem geringsten Abstand zu den Gitterpunkten gewählt und über eine schnelle Online- 
Trajektorienplanung ein Update der Trajektorie aus der Datenbank ausgeführt. In [11] 
wird diese Methode adaptiert, um ein inverses Pendel mit einem KUKA Industrieroboter 
aufzuschwingen. In beiden Arbeiten existiert ein flacher Ausgang, somit vereinfacht sich 
für die Trajektorienplanung das nichtlineare dynamische System jeweils zu einem linearen 
Integratorsystem. 

In dieser Arbeit wird aufbauend auf [10, 11] für ein bekanntes Objekt, dessen Pose 
optisch mittels einer Kamera gemessen bzw. geschätzt wird, offline eine Trajektorie 
berechnet. Während der Roboterbewegung wird die Pose zyklisch neu gemessen und durch 
eine schnelle Online-Trajektorienplanung eine neue Trajektorie berechnet. Für das Greifen 
wird die Greifhand Schunk SDH2 als 2-Finger-Greifer verwendet. Für die Finger wird 
ein kinematisches Modell hergeleitet und eine Trajektorie der Finger für ein erfolgreiches 
Greifen berechnet. 


1.1 Gliederung der Arbeit 


Ziel ist das dynamische Greifen eines Objekts mittels des Industrieroboters KUKA LBR 
iiwa 14 R820. Die dazu notwendigen Schritte lassen sich in einzelne Kategorien aufteilen. 

Im ersten Teil von Kapitel 2 werden die Komponenten vorgestellt, welche für die 
Durchführung des dynamischen Greifvorgangs verwendet werden. Der zweite und dritte 
Teil beschäftigt sich mit der mathematischen Modellierung des Roboters und der Greifhand. 

Kapitel 3 beinhaltet die optische Objekterkennung bzw. Posenschätzung aus den einzel- 
nen Bildern der Kamera. Um den Einfluss von Ausreißern (Outlier) zu verringern und 
gleichzeitig eine bessere Posenschätzung zwischen den einzelnen Bildern zu ermöglichen, 
wird ein Kalman-Filter auf Basis eines Kalman-Filters zur Pose-Estimation mittels IMU 
entworfen. 

Kapitel 4 beinhaltet die Offine- und Online-Trajektorienplanung des Roboters und der 
Finger für ein erfolgreiches Greifen des Objekts. 

In Kapitel 5 werden der Trajektorienfolgeregler des Roboters und der Greifhand ent- 
worfen. 

Die Implementierung des kompletten Setups und die Ergebnisse einiger experimenteller 
Versuche sind in Kapitel 6 dargestellt. 
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2 Modellbildung 


Dieses Kapitel beschäftigt sich mit der Systembeschreibung und der Herleitung der ma- 
thematischen Modelle. Mit den berechneten Modellen können Simulationen durchgeführt 
werden bzw. dienen sie als Basis für die Trajektorienplanung in Kapitel 4 sowie für den 
Entwurf eines Trajektorienfolgereglers in Kapitel 5. 

Im ersten Teil dieses Kapitels werden die verwendeten Komponenten näher vorgestellt 
und gezeigt wie diese zu einem Setup zusammengebaut werden. 

In Abschnitt 2.2 und 2.3 werden die mathematischen Modelle für den Roboter und für 
die Greifhand hergeleitet. Für das dynamische Modell des Roboters werden die Greifhand 
und die Kamera als Punktmasse betrachtet und der Roboter um diese erweitert. 

Um das Greifen eines Objekts mit den Fingern zu planen, wird zunächst zwischen den 
Fingerspitzen ein Greifpunkt definiert. Damit ist jedoch nur die Position des Greifpunktes 
gegeben, womit noch keine Planung eines Griffs möglich ist. Mit der Einführung weiterer 
Zustandsgrößen für die Öffnung der Finger und die Orientierung der Tangente der Greif- 
flächen bei geschlossenem Griff vervollständigt sich der Greifzustand und ein Griff kann 
geplant werden. Über die Vorwärtskinematik wird der Greifzustand aus den Zuständen 
der Finger berechnet. Umgekehrt werden über die Rückwärtskinematik die Zustände der 
Finger aus den Greifzuständen berechnet. Die Herleitung der analytischen Lösung der 
Vorwärts- bzw. Rückwärtskinematik befindet sich im zweiten Teil dieses Kapitels. Für 
die Rückwärtskinematik muss ein zulässiges Gebiet definiert werden, da die Lösung der 
Rückwärtskinematik nicht für alle möglichen Greifzustände existiert. Durch die interne 
Steuerung bzw. Regelung der Greifhand, auf die später in dieser Arbeit in Kapitel 5 und 
6 näher eingegangen wird, ist die Kinematik ausreichend und auf ein dynamisches Modell 
wird verzichtet. 


2.1 Systembeschreibung 


In diesem Abschnitt werden die verwendeten Komponenten der Roboter, die Greifhand, 
die Kamera und das verwendete Greifobjekt näher vorgestellt. In Abschnitt 2.1.1 wird der 
verwendete Roboter KUKA LBR iiwa 14 R820 und die für die Modellierung verwendeten 
Koordinatensysteme (0;2;y;2;),i € {1,...,7} vorgestellt. Abschnitt 2.1.2 beschäftigt sich 
mit der geometrischen Beschreibung des verwendeten Greifers Schunk SDH2. Abschnitt 
2.1.3 zeigt das zu greifende Objekt. Das in dieser Arbeit verwendete Kamerasystem wird 
in Abschnitt 2.1.4 vorgestellt. Für den Versuch werden die Greifhand und die Kamera, 
wie in Abschnitt 2.1.5 gezeigt, als Endeffektor am Roboter montiert. 
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2 Modellbildung 2 Modellbildung 5 


2.1.1 Roboter Kuka LBR iiwa 14 R820 


Der verwendete Roboter KUKA LBR iiwa 14 R820 ist in Abbildung 2.1 dargestellt. 
Der Roboter besteht aus 8 Starrkörperelementen, welche über elastische Drehgelenke 


miteinander verbunden sind. Die Koordinatensysteme (O;2;%2),i € {1,...,7} sind in den 
Drehgelenken platziert. Die Drehung erfolgt jeweils um die z-Achse. Somit verfügt der 
Roboter über sieben Freiheitsgrade, welche in weiterer Folge mit q,,i € {1,...,7} bezeichnet 


werden. Das Koordinatensystem (0oxoYo2o) ist die Roboterbasis und stellt gleichzeitig das 
Basiskoordinatensystem (Oxyz) dar. Das Koordinatensystem (0.2eyeze) ist die Stelle, an 
welcher der Endeffektor montiert wird. Zusätzlich zur Drehwinkelmessung verfügt jedes 
Gelenk über einen Drehmomentsensor, mit welchem das Drehmoment zwischen Motorachse 
und Gelenksachse gemessen wird. Damit können von außen einwirkende Kräfte erkannt 
und darauf reagiert werden. Des Weiteren lässt sich auf Basis von [12] die Singuläre 
Störtheorie anwenden, um ein Regelungskonzept zu entwickeln. Dabei beschreibt das 
langsame Teilsystem die Dynamik des Starrkörpersystems des Roboters und das schnelle 
Teilsystem die Dynamik der flexiblen Gelenke. Die benötigten Parameter des Roboters 
sind in Tabelle A.2 im Anhang angegeben. 


2.1.2 Greifhand Schunk SDH2 


Als Endeffektor des Roboters wird die in Abbildung 2.2 abgebildete Greifhand Schunk 
SDH2 mit drei Fingern verwendet. Jeder Finger besteht aus einem proximalen und 
einem distalen Element, welche durch ein Drehgelenk miteinander verbunden sind. Das 
proximale Element ist über ein weiteres Drehgelenk mit der Basis verbunden, wodurch 
sich zwei Freiheitsgrade pro Finger ergeben. Durch einen weiteren Freiheitsgrad ist eine 
Rotation von Finger 1 und 3 um die Hochachse möglich. Finger 1 und 3 drehen sich dabei 
synchron, aber in entgegengesetzter Richtung. In Abbildung 2.2 sind die körperfesten 
Koordinatensysteme der Basis (On&£nYyn2n) und der Finger (Opitniynizm), i € {1,...,6} 
inklusive der Freiheitsgrade qnı € [0°,90°] und qn; € [-90°,90°] ,i € {2,... ,7} eingezeichnet. 
Die gleichzeitige Drehung der Finger 1 und 3 um die Hochachse erfolgt jeweils um die 
z-Achse des jeweiligen Koordinatensystems. Die Drehung für die Drehgelenke erfolgt 
jeweils um die x-Achse des jeweiligen Koordinatensystems. 

Zum Greifen eines Versuchsobjekts werden in dieser Arbeit nur Finger 1 und Finger 3 
benötigt. Finger 2 bleibt auf der initialen Konfiguration qn4,0 = 90° und gr5,0 = 60° und 
ist in den restlichen Abbildungen dieser Arbeit nicht mehr dargestellt. Die erforderlichen 
Parameter sind in Abbildung 2.3 eingezeichnet und in Tabelle A.3 angegeben. 


2.1.3 Greifobjekt 


Als Versuchsobjekt wird das in Abbildung 2.4 dargestellte Blechbiegeteil verwendet. Die 
Basis des Greifobjekts ist das Koordinatensystem (0.209020). Weiters wird ein Greif- 
punkt definiert, dargestellt durch das Koordinatensystem (O,%gYg2g). Für die Wahl des 
Greifpunktes existieren etliche Algorithmen, die aus den geometrischen Eigenschaften des 
Objekts und den unterschiedlichsten Bewertungskriterien hinsichtlich der Qualität eines 
Griffs den optimalen Greifpunkt berechnen, siehe dazu [13, 14]. Eine Methode ermöglicht 
z.B. die Berechnung der optimalen Positionen der Kontaktpunkte zwischen Finger und 
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Abbildung 2.1: Roboter KUKA LBR iiwa 14 R820 mit eingezeichneten Koordinatensyste- 
men (0;2;y;2;), € {0,...,7,e} und den Freiheitsgraden q, € {1,...,7}. 
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Abbildung 2.2: Greifhand Schunk SDH2 mit eingezeichneten Koordinatensystemen 
(Op£nYnzn), (OnitniYmizm), ? € {1,...,6} und den Freiheitsgraden gn;, 
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Abbildung 2.3: Seitenansicht Schunk SDH2 ohne Finger 2 für q)ı = 90° mit eingezeichneten 
Parametern. 
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Objekt, so dass die wirkenden Klemmkräfte maximiert werden. Eine weitere Methode 
ermöglicht eine Berechnung der Kontaktpunkte, so dass die Widerstandsfähigkeit durch 
externe auf das Objekt einwirkende Kräfte aufgrund der Zentrifugal- bzw. Corioliskraft 
oder einer sonstigen externen Störung maximiert wird. Doch die reine Bestimmung der 
optimalen Kontaktpunkte ist nicht ausreichend. Die Geometrie der Finger des Greifers 
muss ebenfalls berücksichtigt werden, um sicherzustellen, dass keine Kollisionen auftreten. 
Der Einfachheit halber wurde auf eine Implementierung dieser Algorithmen in dieser 
Arbeit verzichtet und manuell ein statischer Greifpunkt definiert. 


Abbildung 2.4: Greifobjekt mit eingezeichneten Koordinatensystemen (0020020) für die 
Objektbasis und (O,24Yg2,) für den Greifpunkt. 


2.1.4 Kamera Basler avA1000 


Für die Objekterkennung wird die GigE-Kamera Basler avA1000-100gc von Abbildung 2.5 
verwendet. Sie liefert Bilder mit einer Auflösung von 1024x1024 Pixel mit einer maximalen 
Bildrate von 100 fps. Die Kamera hat einen globalen Shutter. Dabei werden alle Zeilen 
eines Bildes gleichzeitig belichtet, dadurch entstehen keine Verzerrungen bei sich bewegen- 
den Objekten. Durch die hohe maximale Bildrate sind kurze Belichtungszeiten möglich. 
Dadurch sind schnellere Bewegungen der Kamera möglich, bevor das Bild durch die 
Bewegungsunschärfe verschwommen dargestellt und die Objekterkennung erschwert wird. 
Neben den Standard RGB-Kameras existieren auch RGBD-Kameras, bei denen zusätzlich 
zum RGB-Bild über einen weiteren Kanal auch die Tiefeninformationen zur Verfügung 
stehen. Bei diesen Kamerasystemen ist die Bildrate der RGB-Bilder üblicherweise auf 30 
fps limitiert. 


2.1.5 Versuchsaufbau 


Die Greifhand und die Kamera werden nach dem Eye-in-Hand Prinzip, wie in Abbildung 
2.6 dargestellt, als Endeffektor am Roboter montiert. Durch die Initialkonfiguration des 
Roboters ist die Kamera auf einen definierten Bereich ausgerichtet, in dem das zu greifende 
Objekt aus Abbildung 2.4 platziert wird. Mittels Objekterkennung wird die Translation 
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Abbildung 2.5: Verwendete Kamera Basler avA1000-100gc mit dem Basis Koordinaten- 
system (O.teYeze)- 


und Rotation zwischen dem Koordinatensystem der Kamera (0.x.Yy.2.) und jenem des 
Greifobjekts (0.%0Yo20) geschätzt. Ausgehend von der geschätzten Pose, dem dynamischen 
Modell des Roboters, dem eingeführten Greifzustand, sowie dem kinematischen Modell für 
den Greifpunkt erfolgt die Planung für den dynamischen Greifvorgang. Der zweite Finger 
der Greifhand wird durch das Design des Greifobjekts und die Position des Greifpunktes 
nicht benötigt. Die Greifhand wird somit als ein 2-Finger Greifer eingesetzt. 


2.2 Roboter mit Greifhand und Kamera 


Im folgenden Abschnitt wird das mathematische Modell des Aufbaus aus Abbildung 2.6 
hergeleitet. Die Kamera und die Greifhand werden dabei als Punktmasse modelliert. Die 
genauen Werte der Trägheitsmomente sind nicht bekannt und wurden jeweils über einen 
einzelnen Volumenkörper mit konstanter Massenverteilung abgeschätzt. 


2.2.1 Kinematisches Modell 


Der Roboter stellt eine Kombination von Starrkörpern dar, welche über Drehgelenke 
miteinander verbunden sind. In jedem Gelenk wird, wie in Abbildung 2.1 dargestellt, 
ein Koordinatensystem platziert. Das kinematische Modell beschreibt die Pose des Ko- 
ordinatensystems der Greifhand (Ontnynzn) und der Kamera (0.x.Yycz.) bezogen auf 
das Basiskoordinatensystem (Oxyz) in Abhängigkeit der Gelenkswinkel des Roboters 


T 
a = la... 
Eine Rotation R} zwischen zwei Koordinatensystemen (0;2;y;2;) und (0,2;yj2;) ist über 
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Abbildung 2.6: Versuchsaufbau des Roboters mit montierter Kamera und Greifhand als 
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Multiplikationen der Rotationsmatrizen 


cos(d) -sin(d) 0 
R,s= |sin(b) cos(d) O|, (2.1a) 
0 0 1 
cos($)) 0 sin() 
R.=|o ı o|, (2.1b) 
—-sin(0) 0 cos(6) 
1 0 0 
R,.,= |0 cos(b) -sin(Y) (2.18) 
0 sin(b)  cos(ı) 


definiert [15]. Dabei bezeichnet R,, eine Rotation um den Winkel $ um die z-Achse, 
R.,o eine Rotation um den Winkel 9 um die y-Achse und R,,., eine Rotation um den 
Winkel ı) um die x-Achse. Die Translation zwischen zwei Koordinatensystemen (0;2;y;2;) 
und (0;2;y;2;) ist über den Verschiebungsvektor d? gegeben. Die Vorwärtskinematik 
wird über die homogene Transformation hergeleitet. Dabei lassen sich Rotationen und 
Translationen zwischen Koordinatensystemen (0;2;y:%;) und (0;2;y;2;) durch die homogene 
Transformation 


angeben. Die inverse homogene Transformation ist über 


er; = (ei) = ' z ee 23) 


1 


gegeben. 

Die Greifhand und die Kamera werden als Punktmasse betrachtet und das Robotermodell 
um diese erweitert. Die Position und Rotation der Kamerabasis (0.x2.yc2.) und der 
Greifhand (O,2„Yn2n) in Bezug zum Basiskoordinatensystem (O.x.yz) ergeben sich aus den 
homogenen Transformationen H5 und HR in Abhängigkeit der Gelenkswinkel q zu 


HS; (q) = HIHTHSHIHSHSHTHS, (2.42) 
H/ (q) = HIH/HIHIHSHSHTHE. (2.4b) 


Für die Herleitung des dynamischen Modells werden noch die homogenen Transformationen 
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der Schwerpunkte der einzelnen Starrkörper des Roboters, der Greifhand und der Kamera 


 (q) = ne (2.52) 
ar (q) = H>, (2.5b) 
HP (q) = in Sa (2.5c) 

A (g)=HiH? SHE, (2.5d) 

2 (q) = HIHHeHSRSENS, (2.5e) 
H?® (q) = HÄH?H3H3H3HSH% (2.5f) 
eu (q) = HAH7H?H3H3HSHL Ar (2.58) 
3% (q) = H5H% (2.56) 

0. (q)=HIH," (2.5i) 

j) 


benötigt. Die jeweiligen Rotationsmatrizen und Translationsvektoren für H! in (2.4) und 
(2.5) sind in Tabelle A.1 im Anhang angegeben. 


2.2.2 Differentielle Kinematik 


Die differentielle Kinematik beschreibt den Zusammenhang zwischen der translatori- 
schen Geschwindigkeit v;,i € {b1,b2,...,b7,be,bh} und der Winkelgeschwindigkeit w;,i € 
{b1,b2,...,b7,bc,bh} der einzelnen Starrkörper des Roboters, der Kamera und der Hand in 
Abhängigkeit der Geschwindigkeiten der Robotergelenke q. Für eine einfachere Darstellung 
gilt für die hochgestellten Indizes in diesem Abschnitt i € {b1,b2,... ,b7,bc,bh}. 

Mit den Manipulator Jacobi-Matrizen Jü (q) werden die Winkelgeschwindigkeiten der 
Gelenke q auf vi, und wi, abgebildet [15], entsprechend 


“ _ BR a ae (2.6) 


wo (Ju (q) 
mit 
(Iv), (q) = a (2.7a) 
(I), (9) = . (2.7b) 


Die Vektoren der Winkelgeschwindigkeiten w{, können ebenfalls mit Hilfe der schiefsym- 
metrischen Matrix [15] 


a 0 WE Way 
Ss(w)=Rö(Ri) = | 0 -w (2.8) 
WW 0 


gebildet werden. 
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2.2.3 Dynamisches Modell 


Das dynamische Robotermodell wird mit Hilfe der Euler-Lagrangegleichungen 


d.7a£ıT BEN? 
(&) (5) SET = 


mit der Lagrange Funktion £=T7 -—V hergeleitet, wobei mit £ die Lagrange Funktion 
bezeichnet wird und T die generalisierten Kräfte bezeichnen. Durch T..t werden externe 
und dissipative Kräfte berücksichtigt. Die kinetische Energie 7 und die potentielle Energie 
V aufgrund der Schwerkraft ergeben sich aus Summe der kinetischen und potentiellen 
Energien der einzelnen Starrkörper i € [b1,b2,...,b7,bc,bh} des Roboters, der Kamera 
und der Greifhand im Basiskoordinatensystem, gemäß 


1 NT ; NT + AT % 
= 3 (et) R;E (Ri) wo+m (vi) vi; (2.108) 
i€{b1,b2....,b7,be,bh} 
v= >. mige, dh, (2.10b) 
i€{b1,b2,...,b7,bc,bh} 


mit dem Einheitsvektor der Schwerkraft e, = [0 0 1]", den Massen m; und den 
körperfesten Trägheitsmatrizen I; = diag (I; «x ,Ii,yy;li,22]) im Schwerpunkt der einzelnen 
Starrkörper. Die translatorischen Geschwindigkeiten v; und die Winkelgeschwindigkeiten 
w; sind durch die Beziehung (2.6) gegeben. 

Die Bewegungsgleichungen eines Roboters werden üblicherweise in der Form 


D(qaä+C(ag)ä+g(q=T+ Test van 


angegeben, mit der generalisierten Massenmatrix D (q), der Coriolismatrix C (q,4), dem 
Vektor der Potentialterme g (q), den Gelenksdrehmomenten r und den generalisierten 
externen Kräften Tezı. Die Massenmatrix D (q) ergibt sich durch Einsetzen von (2.6) in 
(2.10a) zu 


T=54 % Kassa)" rin (Rb)" + (A) a] a (2.12) 


ie {b1,b2,...,b7,bc,bh} 


D(q) 


In der Coriolismatrix C (q,4) sind die Zentrifugal- und Coriolisterme enthalten. Die 
Elemente der Coriolismatrix werden über 


C (4,4% — > Cijk (q)di (2.13) 
i=l 


berechnet. Dabei bezeichnen c;;; (q) die sog. Christoffel Symbole, die sich mit den (k,j)-ten 
Elemente der Massenmatrix dx; (q) =D (ri aus 


rn 1 (Ode; (q) , ddri(q) | Adij(q) 
ra = dg; + dgj ” gr oo 
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ergeben. Bei dem KUKA LBR iiwa 14 R820 Roboter handelt es sich um einen Roboter 
mit flexiblen Gelenken. Für solche Roboter ist das Modell (2.11) nicht ausreichend 
und wird auf Basis von [12] erweitert. Ein flexibles Gelenk wird mit einer Drehfeder 
zwischen der Motorachse und der Gelenksachse mit den Federkonstanten K modelliert. 
Das wirkende Drehmoment an der Gelenksachse ist proportional zur Differenz des Winkels 
der Motorachse 9 und des Gelenkswinkels q 


T=K(0-dq). (2.15) 


Die Dynamik der Motorachse des Roboters wird mit der Matrix der Rotorträgheitsmomente 
B und den Drehmomenten der Motoren T über 


Bö+K(-qg)=Tm (2.16) 


beschrieben. Einsetzen von (2.15) in (2.11) ergibt zusammen mit der Rotordynamik (2.16) 
das Modell eines Roboters mit flexiblen Gelenken 


D(ä+C(adats(q=K(d-q)+ Ten, (2.17a) 
Bö+K(-qg)=Tm. (2.17b) 


Auf Basis der singulären Störtheorie kann gezeigt werden, dass es sich um ein schnelles 
und ein langsames Teilsystem handelt. Zusätzlich zu den Zustandsgrößen q und q stehen 
beim verwendeten Roboter auch die Drehmomente der jeweiligen Gelenke als Messgrößen 
T, zur Verfügung. Aus diesen Messgrößen wird über ein Differenzierungsfilter 7, gebildet. 
Durch das PD-Zustandsregelgesetz 


Tm=Ta-Kı (Ts - Ta) - eDrts (2.18) 
ergibt sich schließlich der geschlossene Kreis zu [12] 


(D()+(E+K,)"B)ä+C(qQ)ä+g(a)=ra. (2.19) 


Dabei bezeichnet E die Einheitsmatrix, K, und D; sind Verstärkungsmatrizen des Reglers 
der singulären Störtheorie und r4 das gewünschte Drehmoment aus einem Regler für das 
Starrkörpermodell. 


2.3 Greifhand Schunk SDH2 


Im folgenden Abschnitt wird die Vorwärtskinematik der Greifhand hergeleitet. Für den 
Greifpunkt an der Hand wird ein zusätzlicher Punkt G definiert, siehe dazu Abbildung 
2.7. Wird eine Linie zwischen den Mittelpunkten C} und C’3 der Rundung der Greiflläche 
gezeichnet, liegt der Punkt G auf dem Mittelpunkt dieser Verbindungslinie. Für eine bessere 
Planung des dynamischen Greifens wird zusätzlich zum Punkt G ein Greifzustand für die 
Distanz zwischen den Greifllächen der Fingerspitzen und einer für die Orientierung der 
Tangente der Greiflläche bei geschlossenen Fingern eingeführt. Durch die Einführung dieser 
Greifzustände wird eine intuitivere Planung des Greifvorgangs ermöglicht. In Abschnitt 
2.3.2 wird die inverse Kinematik für die Berechnung der Zustandsgrößen der Greifhand 
aus den Greifzuständen hergeleitet. Weiters wird in Abschnitt 2.3.3 ein zulässiges Gebiet 
für die Lösbarkeit der inversen Kinematik definiert. 
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2.3.1 Vorwärtskinematik 


Wie bereits erwähnt, wird der Finger 2 für das Greifen des Objekts nicht benötigt. Die 
Zustandsgrößen des Fingers 2 werden auf die konstanten Werte qna = Qna,o und qn5 = 915,0 
gesetzt und somit wird der Greifvorgang durch den Finger 2 nicht behindert. Da der 
Greifvorgang mit Finger 1 und 3 ausgeführt wird, muss die Drehung der beiden Finger um 
die jeweilige Hochachse konstant auf den Wert qnı = Qnı,o = 90° gesetzt werden, damit 
die Greifllächen der beiden Finger parallel zueinander sind. 

Die Vorwärtskinematik pS’ (q„) beschreibt die Position des Punktes G in Abhängigkeit 
der Gelenkskoordinaten qr. Zur besseren Lesbarkeit wird im Weiteren auf die Argumente 
q,„ verzichtet. In diesem Abschnitt bezeichnet der Vektor p' eine Translation und die 
Matrix R' eine Rotation zwischen zwei Koordinatensystemen (0,274j2;) und (Op&kYyr2r)- 

Die Translationen zwischen den Koordinatensystemen der Hand (Opitniynizmi),i € 
{1,2,...,6} mit der Basis (O,„Yn2n) ergeben sich mit den in Abbildung 2.3 dargestellten 
Parametern zu 


pr! == 1, -51,h1]" ’ ph = [0, 0,ha]" ’ Phs — [0, sc sin 0,5 cos 017 ’ (2.20a) 
T 

pP=[-h0h), Deep Di=sDo, (2.20b) 
T 

p? = [h,b1,h1]" , ph =pir, Die =D: (2.20c) 


Die Freiheitsgrade der Hand gn;,i € {1,2,...,7} sind in Abbildung 2.2 und 2.7 eingezeich- 
net, daraus ergeben sich die Rotationsmatrizen 


—1 0 0 
RR = R,,-2-q1Ranıae|_ =|0 -cos(gp) sSin(gp)|; (2.21a) 
Ir1 41,0 R 
0 sin(gn2)  cos(gn2) 
E 0 0 
Ri =R,,.0; = |0 cos(gn3) —sSin(qns)|, (2.21b) 
0 sin(gqn3)  cos(gn3) 
[0 0 1] 
Ri 7 R,, ‚= Rons,ana qna=qna = |1 0 0 ’ (2.21c) 
°» /010 
1 0 0 
Ry3 = Ronasans Igns=ans 0 = [0 3 + ’ (2.21d) 
3 1 
05% 59 
E 0 0 
Rj’ = R,,-3+q1 Rans.ans een = /0 cos(q16) — sin(gn6) | ; (2.21e) 
0 sin(gqne)  cos(gne) 
1 0 0 
Ri = Ray, = |9 cos(gnr) -sin(gp7)|; (2.21£) 
0 sin(gqnr)  cos(gn7) 


wobei für gp1, gnı und gn5 die Initialwerte eingesetzt werden. Die Position der Koordina- 
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tensysteme der Fingerspitzen ist gegeben durch 


pi? = ph! + Rip? = [I1, — bi + h2sin (qn2) ‚hı + hacos (qn2)]" , (2.22a) 
pi — p> En RFSpRE — [1 ,bı _ ha sin (Ans) ‚hı + ha cos (qr6)]" (2.22b) 


und die Position der Zentren der Rundungen der Fingerspitzen durch 


Ph = pi + Ru Ryipız (2.23a) 

lı cı 
= |-b1 + hasin (qn2) + sc sin (g9R2 + Qn3 — 9)| = |vcı| ; (2.236) 

hı + h2cos (qn2) + sc cos (qn2 + Ana — 0) 201 
PR = pi + RIRUSPIG (2.24a) 

hı | Tees] 
= | bi — hasin (gn6) — sc sin (qn6 + qnr — ®) | = |yca| - (2.24b) 

hı + h2.cos (qh6) + Sc cos (Qn6 + Ahr — 0) 203 

Die Position des Greifpunktes G berechnet sich aus 
C3 2 
Pn —-P 

pe=pi + aa. (2.25) 


2 


2.3.2 Inverse Kinematik 
Für die Planung eines Griffs werden vier neue Zustandsgrößen qG = [de, 26;4G, ze)" 
eingeführt, durch welche der Greifzustand beschrieben wird. Sie sind in Abbildung 2.7 
dargestellt. Dabei bezeichnen yg und zc die y- und z-Komponenten des Greifpunktes G 
in Bezug auf das Koordinatensystem (OntnYyn2zn)- PG beschreibt den Greifwinkel und da 
definiert wie weit die Finger geöffnet sind. In diesem Abschnitt wird die inverse Kinematik 
hergeleitet. Damit lassen sich die Zustandsgrößen gn>2, Qn3, Qng und gn7 in Abhängigkeit 
der Zustandsgrößen qc berechnen. Zur besseren Lesbarkeit wird im Weiteren auf die 
Argumente qc verzichtet. 

Die y- und z-Komponenten der Mittelpunkte der Rundungen der Fingerspitzen C} und 
Cs ergeben sich mit den Greifzuständen q« zu (siehe Abbildung 2.3 und Abbildung 2.7) 


d 

ucı = vo = (rr2 + &) 005 (po), (2.26) 
d . 

201 =26 - (7 + =) sin (26) , (2.27) 
de 

yo =ya+|rr2+ 7) cos (#0) ; (2.28) 
de\ . 

203 =2c+\rm+ D sin (pG): (2.29) 


Um einen Ausdruck für gn2 bzw. qng zu erhalten, werden jeweils die y- und z-Komponenten 


je) ES 

- [ey 

3 s 

3 5 
"I n 

= Ö 18 9% 

=. ve 

— o Ss 

e= S & 

S) $) fe} 

= > |\g N 

S {| L 5 

£ Re 

| : 

I ı [a 5 I Tr B= 

0 3 Ve 18 o 

os ı- 

| © 

ee 9 Fe © 2993 N) N 

| eb} 

r © 

| = 

| 1} . 

S je © 

[\ 

© 3 

"3 T 

= = 

ine \:@ :z 

er Fe = 

SS Q T 

= 

En ® 

13 8 

’ [} 

Pan 

\@) 

e H— en 

13% 

- 

3 

- S [| I 

oO FE: 

! = 

© 

= 

© 

&n „ 2 

S 8 Re 

> °< nn 

= a 

= 2 

< |, 8 

S Be 

He) 

= a u u Te mr SumeE: Som mer me: Fame re ee me ME 

N = Ss Ss Ss Ss 2 Ss Ss Ss Ss Ss s Ss <« 


Yayloıgıg usıM NL Ye uud uı sjgejtene sı sısayı sıy) Jo UoIsIan jeulßl1o panoldde syı 
Jegßnyuan Yayıoıgıg ualM NL Jap we ısı naquewojdıq Jasaıp uoIsisnfeuißluo apjanıpaß auaıgoldde sıq 


und dem Greifzustand ga. 


any 33pa]mouy ınoı NIEREN 
GIayNoNgIeE Mi 


Die approbierte gedruckte Originalversion dieser Diplomarbeit ist an der TU Wien Bibliothek verfügbar 


The approved original version of this thesis is available in print at TU Wien Bibliothek. 


iothek, 


3ibl 


LAN, Your knowledge hub 


TU 


2 Modellbildung 2 Modellbildung 19 


von (2.23b) und (2.24b) quadriert, addiert und alle Terme auf die linke Seite gebracht, 
entsprechend 


2 (hı — zcı) ha cos (qn2) — 2 (ycı + bı) hasin (gn2) + 


(2.30a) 
b7 + 2bıycı + hl — 2hızcı + hd + yaı + zer - 50 = 0, 
2 (hı — zc3) ha cos (qn6) — 2 (bı — Yca) hasin (qn6) + (2.306) 
b? — 2b1yoa + h? — 2hızaz3 + h2 + Yang + 203 = 8% —N. j 
Zusätzlich werden folgende Substitutionen 
hi . 2t; i-=3 . 
t; = tan (=) , sin(gu) = 7 Eu; cos (qm) = n rer; ie {2,6} (2.31) 
und 
eı = -2(ycı + bı) ha, (2.32a) 
ea = 2 (hı = zcı) ha, (2.32b) 
eg = b? +2b1ycı + hı — 2hızcı + h2 _ s2, + Yen + 2; (2.32c) 
e4 = —2(bı — yca) ha, (2.32d) 
e5 = 2 (hı — 203) ha, (2.32e) 
es = br — 2biyoz + hr — 2hızca3 + Ar _ Bi + Yeız + 26 (2.32E) 


eingeführt. Einsetzen von (2.31) und (2.32) in (2.30) führt zu den quadratischen Gleichun- 
gen 


2% + 
e 2 lie 2 g=0 (2.33a) 
geR} °1+12 37% . 
Im -t2 
a 2.33b 
era ( ) 


Die Lösung dieser quadratischen Gleichungen ist 


=, je + e2 Zi e2 
to, , = : 2.34a 
21,2 er ( ) 
-e4zE Je? + B- = e2 
t = ; 2.34b 
61,2 este ( ) 


Für den Tangens von gp2 und gn6 existieren zwei Lösungen. Um einen möglichst großen 
Winkel zwischen den proximalen Teilen der Finger zu erreichen, wird die größere Lösung 
der quadratischen Gleichung für die Berechnung der Winkel ga und gng verwendet, gemäß 


-a+/d+&-e& 
oe (2.35a) 


gn2 = —2arctan 
e2 — e3 


-4u+t/d+te-e 
Gn6 = —2arctan SE ze (2.35b) 
e5— &6 
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Einsetzen von (2.35) in die y„-Komponente von (2.23b) und (2.24b) ergibt 


i ha -bı - 
ee (® (qn2) ha — bı za | Be 
sc 
i hab 
dh7 = —Gn6 + 0 — arcsin (= (ans) ha 1. ver) (2.366) 
sc 
Zusammengefasst in einem Vektor ergibt sich die inverse Kinematik zu 
dh1,0 
—eı+1/e?+e2-e2 
—2arctan en 
—qgna + 0 — arcsin (see) 
sc 
qn (4G) = dnA,O 3 (2.37) 
dh5,0 
_9arctan | Stv eat 
es—eg 
—Gn6 + 0 — arcsin (ee) 


wobei gn1,0, Qra,0o und gn5,0o als konstant angenommen werden. 


2.3.3 Zulässiges Gebiet 


Nicht für alle Werte von qc existiert eine Lösung der Rückwärtskinematik. Zwei Begren- 
zungen ergeben sich aus der Lösung der quadratischen Gleichung (2.34). Um ein reelles 
Ergebnis zu erhalten, müssen die Terme unter den Wurzeln die Bedingungen 


-2-&2+&<0, -3-&g+&<0 (2.38) 


erfüllen. Die maximale Höhe des Greifpunktes za, bezogen auf die Basis der Hand 
(On&£nynzn), wird durch die oberen Enden der Greifflächen der Fingerspitzen limitiert. Es 
werden, wie in Abbildung 2.8a eingezeichnet, an den oberen Enden der Greifflächen die 
Punkte 7} und 73 mit den Ortsvektoren prı und pr3 gemäß 


Ih 
prı = |—bı + hasin (gp2) + sr sin (qn2 + qns + n)| ; (2.39a) 
hı + h2cos (qn2) + st cos (qn2 + na + 7) 
Iı 
Pr3 = | bı - hasin (gn6) — sr sin (Ans + Inr + N) (2.395) 
hı + ha cos (gn6) + ST cos (qn6 + nr + N) 


eingeführt. Für einen erfolgreichen Griff muss za gleich oder kleiner sein als die 2- 
Komponenten der Vektoren prı und pr3. Daraus ergeben sich die Ungleichungsbeschrän- 
kungen 


—hı — ha cos (qn2) — sr cos (2 + ma +n)+2zc <d, (2.40) 
—hı — h2 cos (gn6) — ST cos (ns + ar +) + ze <d. (2.41) 
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h6Xn6Yn62h6) 


(a) Seitenansicht mit den Punkten T\ und 7; (b) Winkel a 


Abbildung 2.8: Darstellung der Punkte 7} und 73 und des Winkels «a. 


Die letzte Gleichung für das zulässige Gebiet ergibt sich aus dem Winkel zwischen den 
beiden ebenen Greifllächen der Fingerspitze. Wird gedanklich das Koordinatensystem 
(OnaXnayn2zn2) in das Koordinatensystem (On6&n6Yyn6Zn6) verschoben, siehe Abbildung 
2.8b, dann muss der Winkel & > 0 sein. Zusätzlich wird für « ein Maximalwert von 
90° festgelegt, womit gilt a € [0,90°]. Aus der Transformation der Einheitsvektoren e; 
der beiden Koordinatensysteme (OnaXnayn2Zzn2) und (OnstnsYynszns) in die Basis der Hand 
ergeben sich die Vektoren e, „2 und e,ng Zu 


e,,n2 = Ri Rile-, (2.42) 
e,,n6 = Ri Rise. (2.43) 
Aus Bildung der Norm des Kreuzproduktes zwischen den beiden Vektoren ergibt sich 


hlph2 h5p h6 
IR R,ie: x R,, R)5e; 


e sin (gr2 + qn3 + Ins + Gar) =sina >0. (2.44) 
Damit beschreibt die Ungleichung 


— sin (gR2 + Qn3 + Qn6 + qnr) < 0 (2.45) 


eine weitere Beschränkung für das zulässige Gebiet. Zusammengefasst wird das zulässige 
Gebiet durch die Ungleichungen 


-—e? = e2 + e2 
-d-ed+e 
ha (Qn,9G) = |-hı — h2cos (gn2) — ST cos (Qn2 + ns +n)+zc| <O (2.46) 
—hı — h2 cos (qn6) — ST COS (qn6 + nr +) + zc 
— sin (Qn2 + Qn3 + Qn6 + Ihr) 


definiert. 
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3 Objekterkennung 


Dieses Kapitel beschreibt die kamerabasierte Schätzung der Pose des Greifobjekts. In 
Abschnitt 3.1 wird die Bestimmung der Pose anhand eines Kamerabildes und mit Hilfe von 
ArUco-Markern erläutert. Diese werden geeignet an bekannten Positionen des zu greifenden 
Objekts angebracht. Um die geschätzte Pose zu glätten und die Roboter-Bewegung zu 
berücksichtigen, wird im zweiten Teil ein Kalman-Filter entworfen. 


3.1 Kamerabasierte Posenschätzung 


Als Modell für die Kamera wird das Pinhole Kamera Modell in Abbildung 3.1 verwendet 
[16]. Dieses Modell beschreibt wie ein Punkt P mit den homogenen Koordinaten P = 
[2i, 91, 2,1)" in einem beliebigen Koordinatensystem (0;x;y:2:) auf den Punkt P mit den 
homogenen Koordinaten B = [u,v,1]" in der Bildebene (Ouv) abgebildet wird, gemäß der 
homogenen Transformation 


sB=K |Rjlt| P. (3.1) 
Dabei ist K die Kamera-Matrix 
[fr 0 ca] 
K=I10 , % (3.2) 
| 0071 | 


mit den intrinsischen Parametern der Kamera fr, fy, Cx und c,, welche die Koordinaten 
im Kamerakoordinatensystem (O.x.yez.) in die Bildebene projiziert. Die Werte der intrin- 
sischen Parameter sind in Tabelle A.4 im Anhang angegeben. Die Matrix [R/|t{] bildet 
sich aus der Rotation R{ und Translation t? zwischen dem Kamerakoordinatensystem 
(Octeyeze) und dem Koordinatensystem (O;t;yi2;) zu 


u rı ra rı3 
[Rijti] = (Irı r22 723 ty (3.3) 
r3ı T32 733 T, 


Der Skalierungsfaktor s errechnet sich aus dem Skalarprodukts der homogenen Koordinaten 
P und der letzten Zeile von (3.3) zu 


s=r31%; + r323Yi + r332; + tz. (3.4) 


Schließlich ergibt sich die 3D-to-2D Projektion zu 


Li 


u 1 0 & sh Ian 
v|=.|0 5% [Räjti] “ (3.5) 
1 0071 1 

22 
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Ye 


79 ar 


Pinhole 


Bildebene 


Abbildung 3.1: Pinhole Kamera Modell. 


Die Werte u und v in (3.5) sind die Pixel-Koordinaten des Punktes P in der Bildebene. Ziel 
ist es, aus einem Kamerabild die Pose [RÄ|tö] zu bestimmen. Für diese Aufgabe existieren 
unterschiedliche Implementierungen des Perspective-n-Point Algorithmus, welche n > 3 
3D-to-2D Projektionen entsprechend (3.5) benötigen. In dieser Arbeit wird der Efficient 
Perspective-n-Point, kurz EPnP, Algorithmus verwendet [2], welcher für n > 4 3D-to-2D 
Projektionen entsprechend (3.5) angewendet werden kann. 

Die Bestimmung der Pose des Greifobjekts [R2|t2] erfolgt mit Hilfe von ArUco-Markern, 
siehe Abbildung 3.2, die an definierten Positionen auf dem Greifobjekt angebracht sind. 
Somit sind die Positionen aller Eckpunkte der einzelnen ArUco-Marker definiert. Zu- 
sätzlich ist jedem Marker durch die unterschiedliche Anordnung der weißen Quadrate 
im Inneren eine eindeutige ID zugewiesen. Damit können für jeden der vier Eckpunkte 
von n sichtbaren ArUco-Markern auf einem Bild die Punktepaare (B;,P;),i € {1,...,4n} 


erstellt werden. Weiters werden die Punkte aus diesen Punktepaaren zu den Matrizen 
= = a T 
p= Dis: ‚Ban]" und P = [Pı, Bee Be zusammengefasst. Die Objektpose in Bezug 


auf das Kamerakoordinatensystem berechnet sich schließlich über den EPnP Algorithmus 
zu 


[R?|t°] = EPnP (B.P) (3.6) 


Abbildung 3.2: ArUco Marker mit der ID 1 bis 3. 
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3.2 Extended Kalman-Filter 


Machine Vision Anwendungen sind sehr rechenintensiv. Das Auslesen des Bildes, identifi- 
zieren der ArUco-Marker und die Berechnung der Pose benötigt etliche Millisekunden. Da 
sich die Kamera am Endeffektor befindet, verändert sich die geschätzte Pose zwischen 
zwei Kamerabildern durch die Bewegung des Roboters. Während der Roboterbewegung 
wird, aufbauend auf der geschätzten Pose, zyklisch ein Trajektorienupdate des Roboters 
nach der in Kapitel 4.3 angeführten Methode ausgeführt. Der Startzeitpunkt des Trajekto- 
rienupdates ist dabei unabhängig von der Posenschätzung. Zusätzlich ist die Zyklusdauer 
für die einzelnen Trajektorienupdates nicht konstant. Somit ist nicht definiert, zu welchen 
Zeitpunkten die geschätzte Pose zur Berechnung des Trajektorienupdates verwendet wird. 
Um die Roboterbewegung zwischen zwei Kamerabildern zu berücksichtigen und damit die 
Posenschätzung für das Trajektorienupdate zu verbessern, wird ein Extended Kalman- 
Filter entworfen. Die Zustandsschätzung erfolgt anhand der Roboterbewegung und mit 
der berechneten Pose aus dem Kamerabild wird ein Messupdate durchgeführt. 

Für den Entwurf des Extended Kalman-Filters wird zunächst ein nichtlineares zeitkon- 
tinuierliches Systemmodell für die Rotation und Distanz zwischen Objekt und Kamera 
hergeleitet. Dieses zeitkontinuierliche Modell wird im nächsten Schritt über das Euler- 
Verfahren in ein nichtlineares Abtastsystem überführt, auf dessen Basis schließlich das 
Extended Kalman-Filter entworfen wird. Die Herleitung der Systemmodelle und der 
Entwurf des Extended Kalman-Filters wird auf Basis von [17] durchgeführt. 

Die Rotation zwischen zwei Koordinatensystemen (0;2;y;%) und (0;2;yj2;) lässt sich in 
Form eines Einheitsquaternions mit 


cos (5) Ba 
| ı Iell = 1 & 


beschreiben [18]. Dabei bezeichnet & die Drehung um die Achse n. Durch das Einsetzen 
von (@a=Y,n=e,), (@=#,n=e,) und (@a=d,n=e,) ergeben sich die elementaren 
Rotationsmatrizen aus (2.1). Die Hintereinanderausführung zweier Rotationen wird über 
das Quaternionprodukt ®& der entsprechenden Quaternionen beschrieben. Dabei ist das 
Quaternionprodukt zweier Quaterionen r und r über 


_ roro - Y1:3°T1: 
ror= B 0 0 1:3 1:3 . (3.8) 
ror1:3 + ror1:3 — T1:3 X T1:3 
definiert. Die Operationen rı:3 -Tı:3 und rı:3 X Tı:3 bezeichnen dabei das Skalarprodukt 
bzw. das Kreuzprodukt im euklidischen Raum. Eine Rotationsmatrix 


rı rı2 r13 
R= Ir r22 723 (3.9) 
r3ı T23 733 
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und ein Quaternion r werden über die Beziehungen 


vrutratrz +1 
ul sign (r32 = )yru Sara t+l (3.104) 
2 |sign (rı3 - raı) Vra—r33 -rıı +1 
sign (rz2ı — rı2) Vr33 -rıı -ra+1 
2 (r? + r7) —1 2(rıra—ror3) 2(rıra + rora) 
R = |2(rıra+rorz) 2(r$+r3)-1 2(rıra + roro) (3.10b) 
2(rır3 -rora) 2(rar3—rorı) 2 (r2 + r2) —1 


ineinander umgerechnet [18]. 
Die Orientierung des Objektkoordinatensystems (0.2%0Yo20) in Bezug auf das Kamera- 
koordinatensystem (O.XcYyeze) lässt sich über das Quaternion 


cos (2) cos 8 cos ($) 
in (® 
i- i See \ 5 (3.11) 
0 sın 8) 
n(% 
sin (3) ) ) 
angeben. Die zeitliche Änderungsrate des Quaternions ergibt sich mit dem Vektor der 


Winkelgeschwindigkeiten des Kamerakoordinatensystems (O.x.Ye2c) we = [we x, We Be 
zu 


. 1 0 
Fe = zre ® u (3.12) 
welche mit der schiefsymmetrischen Matrix 
0 We,x Wey We,z 
w 0 w w 
Ds) . = 3.13 
( c) Wey Wez 0 Wex ( 
Wez  Wey We,x 0 
auch über i 
= „rwe)re (3.14) 


angegeben werden kann. Die Winkelgeschwindigkeiten wer, we,, und w.,, sind die momen- 
tanen ortsfesten Drehwinkelgeschwindigkeiten um die x-, y- und z-Achse. 

Um die Pose des Objektkoordinatensystems (0020020) in Bezug auf das Kamerako- 
ordinatensystem (0.x.Yy.2z.) zu vervollständigen, wird der Distanzvektor d? und dessen 
zeitliche Ableitung d? definiert. Die Distanz zum Objekt im Basiskoordinatensystem ergibt 
aus der homogenen Transformation HS; (2.4a) und dem konstanten, jedoch undefinierten 
Positionsvektor d‘ des Objekts im Bezug zum Basiskoordinatensystem zu d? = -d& +d} 
mit der zeitlichen Ableitung d? = -d® = -v.. 

Die Geschwindigkeiten w. und v. werden aus transformierten Sensorgrößen &. = 
(RS) wo und v.= vo abzüglich einer stochastischen Störung w, und w, über 


& = Be - e wo = (3.15) 
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gebildet. Die Messwerte der Geschwindigkeiten @5 und v5 in (3.15) bilden sich über 
die Beziehung (2.6) und (2.7) aus den über Sensorgrößen bestimmten momentanen 
Gelenkswinkelgeschwindigkeiten g des Roboters. 

Damit ergibt sich das Systemmodell des Extended Kalman-Filters mit dem Zustand 


x = [(xe)" ; (de)"] ER’, dem Systemeingang u! = (#7, v7] € R®, der stochastischen 


Prozessstörung w" = [w2, wi E R®, dem Ausgang y’ = (re)? 3 (de)") e R’ und dem 


stochastischen Messrauschen ve R’ zu 


j 19(&.- wu r? 
x=flx,uw) = i S A . x RO) (3.16a) 
chen N +vV. (3.16b) 


Das Extended Kalman-Filter wird auf einem Digitalrechner mit der Abtastzeit 7, imple- 
mentiert. Unter der Annahme einer kleinen Abtastzeit, und dass zwischen zwei Abtast- 
zeitpunkten kT,„ <t <(k+1)T, die Eingangsgrößen konstant mit u(t) = u(kT,) = ur 
sind, ergibt sich aus (3.16) und dem Euler-Verfahren das nichtlineare Abtastsystem 


xk+1 = F (xXr,U%,wr) =xk +f (Xr,UR, wer) Ta; x(0)=%o (3.17a) 
yk=h(x, ve). (3.17b) 


Um die Iterationsgleichungen für das Extended Kalman-Filter herzuleiten, wird zunächst 
für die rechte Seite der Systemdynamik (3.17a) eine Taylor-Reihenentwicklung um den 
Punkt x; = Re ur; = ur und w; = 0, sowie für die rechte Seite der Ausgangsgleichung 
(3.17b) eine Taylor-Reihenentwicklung um den Punkt x, = X, und v; = O0 durchgeführt, 
und jeweils nach dem linearen Term abgebrochen. Daraus resultiert das linearisierte 
Abtastsystem 


%41 = PıX%r + Ur + Gew (3.18a) 
yr; = CıHx; + U: + v2. (3.18b) 


mit den Systemmatrizen ®, € R’”*7, G, € R’*6 und C, € R’X”, der nx n Einheitsmatrix 
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E,„xn und den Vektoren ü, € R’ und ü, € R’ gemäß 


5+ 
„,_ FEo) _ Eixa + Tau. .:3) O4x3 (3.19) 
Oxk 03x4 E3x3 
X.o x%,3 Ka 000 
x1 —Xy4 x3 000 
Or (&,,0) | Ka Hi Rn 000 
un Ow; — 3 2 %ı 0000|, (3.19b) 
0 0 0.200 
0 0 020 
0 0 0.002 
öh (x ,0 
oz an = Emm (3.19c) 
xXk 
ü, = F(&},u,0) - Sixt, (3.194) 
ü, = h(8,,0) - Cu$;. (3.186) 


Dabei bezeichnet x;,,; bzw. uz;,; das ite Element des Zustandes x;, bzw. des Eingangs u;.. 
Für die partiellen Ableitungen in (3.19) gilt jeweils die vereinfachte Schreibweise 


OF (*},u4,0) OF (Xr,Ux, Wr) 


Di Öx, (3.20) 


+ = = 
Xk=X, UR=UR, WE =O 


Weiters wird für die Störung w;. und das Messrauschen v,, vorausgesetzt, dass mit dem 
Kroneckersymbol ö;; =1 für k = j und öx; = 0 sonst gilt 


E (v;) =0 E (vev?) — R,ör; (3.21a) 
E(w.) =0 E (ww!) = Qxök, (3.21b) 
E (wiv?) = 0. (3.21c) 


Die Werte von R, und Qı sind in Tabelle A.5 im Anhang angegeben. 

Die gemessene Ausgangsgröße yj, ist die über den EPnP Algorithmus berechnete 
Pose [R2|t2] aus (3.6), wobei die Rotationsmatrix R2 über (3.10) in das Quaternion r? 
umgerechnet, sowie der Vektor t? im Kamerakoordinatensystem über d? = Röt2 ins 
Basiskoordinatensystem transformiert wird. 
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Das Kalman-Filter ist über die Iterationsgleichungen 


N -1 
i,=P;C] (C,P;CT+R,) (3.222) 
=, +La (vi = Cı&;) (3.22b) 
Pf = (E-L,C,)P; (3.22c) 
OF (X ,u;,0) 
®, = 22 
k Dr (3.22d) 
ÖF (Stu, 0) 
= D 
Gr no (3.22e) 
%,, = F(&,w,0) (3.22f) 
Pr. = Pf ®, + GrQıGE (3.228) 


gegeben mit der a priori Schätzung des Zustandes X, und der Fehlerkovarianzmatrix 
P,., der a posteriori Schätzung des Zustandes = und der Fehlerkovarianzmatrix Pr} 
und der Kalman-Verstärkungsmatrix bi Zu Beginn der Messung wird das Kalman- 
Filter mit dem gemessenen Zustand X9 = y; und der Fehlerkovarianzmatrix Po = E7x7 
initialisiert. Da zwischen den einzelnen Messwerten etliche ms vergehen und die Abtastzeit 
T. wesentlich kürzer ist, wird das Update der Zustandsschätzung (3.22b) sowie das Update 
der Fehlerkovarianzmatrix (3.22c) nicht in jedem Iterationsschritt durchgeführt, sondern 
nur wenn ein neuer Messwert yr, aus (3.6) zur Verfügung steht. Für die Iterationen 
zwischen zwei Messwerten y; gilt 3er = x, sowie Pr =P,. 

Das Kalman-Filter liefert die geschätzte Translation t? = (RS)T d° und mit (3.10) die 
geschätzte Rotation R°. Die geschätzte Pose des Objekts im Bezugskoordinatensystem 
ergibt sich schließlich zu 


To _ Rs a6 R? = RER? No _ ac Rt? 3.23 
0o= 0 = oR. oda + Hot... (3.23) 


Mit der geschätzten Pose (3.23) des Objekts wird im nächsten Kapitel eine Trajektori- 
enplanung durchgeführt. 
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4 Trajektorienplanung 


Dieses Kapitel beschäftigt sich mit der Berechnung der Trajektorien q, q und ä des 
Roboters sowie gr, 9n und än der Greifhand auf Basis von [10, 11]. Die Trajektorien 
des Roboters und der Greifhand werden über eine Offlliine-Trajektorienplanung berechnet. 
Über eine Online-Trajektorienplanung wird während der Roboterbewegung ein Update 
der Robotertrajektorie durchgeführt. Die Ofline- bzw. Online-Planung erfolgt optima- 
litätsbasiert auf Basis eines zeitkontinuierlichen Integratorsystems, aus welchem durch 
Diskretisierung ein statisches beschränktes Optimierungsproblem formuliert wird. Durch 
Interpolation der Lösung des beschränkten statischen Optimierungsproblems wird eine 
zeitkontinuierliche Trajektorie berechnet. 

Vor der Offline-Trajektorienplanung wird in Abschnitt 4.1 der optimale Griff berechnet. 
Aufbauend auf dem optimalen Griff erfolgt eine Offline-Trajektorienplanung der Finger in 
Abschnitt 4.2 und anschließend eine OfHine-Trajektorienplanung des Roboters in Abschnitt 
4.3. Die gesamte Robotertrajektorie setzt sich aus drei einzelnen Trajektorien zusammen, 
eine Trajektorie für die Bewegung während des Schließens der Finger in Abschnitt 4.3.1, 
eine für die Bewegung von der Initialposition zum Objekt in Abschnitt 4.3.2 und eine für die 
Bewegung zur Zielposition nach dem Greifen des Objekts in Abschnitt 4.3.3. In Abschnitt 
4.4 werden die berechneten Trajektorien interpoliert. Während der Roboterbewegung 
wird zyklisch die Pose des Objekts über das Kalman-Filter geschätzt. Anhand dieser 
Schätzung wird in Abschnitt 4.5 ein online Update der Robotertrajektorie aus Abschnitt 4.3 
durchgeführt. Nach jedem online Update erfolgt erneute eine Interpolation der Trajektorie 
gemäß Abschnitt 4.4. 

Die zeitliche Abfolge eines Greifvorgangs ist in Abbildung 4.1 dargestellt. Im blauen 
Bereich mit t € [0,7T}] bewegt sich der Roboter von einer Initialposition gg zum Objekt. 
Das Greifen des Objekts findet im roten Bereich mit t € [71,71 + Ta] statt. In diesem 
Bereich wird zusätzlich zur Fingerbewegung mit dem Roboter eine Schwenkbewegung der 
Greifhand durchgeführt. Im grünen Bereich mitte N, +7,7]ud7’=T, +7, +T3 ist 
der Greifvorgang beendet und der Roboter bewegt sich zu einer Zielposition qr. 


U SGes—,—,———— 
0 Tr nTı+n T t 


Abbildung 4.1: Zeitlinie des gesamten Greifvorganges. 


Abbildung 4.2 zeigt eine Beispielkonfiguration für die Planung einer Trajektorie. Es sind 
die Koordinatensysteme der Hand, der Kamera, des Objekts, des Greifpunktes am Objekt 
und das Basiskoordinatensystem eingezeichnet. Die nicht beschrifteten Koordinatensysteme 
sind die Koordinatensysteme in den Gelenken des Roboters. Zusätzlich sind die homogenen 
Transformationen H$ zwischen dem Basiskoordinatensystem und der Kamera, HR zwischen 


29 
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dem Basiskoordinatensystem und der Greifhand, HI zwischen dem Koordinatensystem 
des Objekts und des Greifpunktes, Ho zwischen dem Koordinatensystem der Kamera und 
des Objekts sowie H3 zwischen dem Basiskoordinatensystem und des Objekts angegeben. 
Bei den mit “ gekennzeichneten homogenen Transformationen Ho und H3 handelt es sich 
um geschätzte Größen auf Basis der Schätzung des Kalman-Filters. Im weiteren Verlauf 
des Kapitels werden diese als bekannt angenommen und mit H2 und Hß bezeichnet. 

Ausgangspunkt der Offliine-Trajektorienplanung der Greifhand in Abschnitt 4.2 sowie 
für den Roboter in Abschnitt 4.3 ist das lineare, zeitinvariante und zeitkontinuirliche 
Integratorsystem mit den gewünschten Zustandsgrößen x — [af | e R?" und dem 
gewünschten Eingang ug = da € R” in Systemdarstellung 


Onxn Onxn Enxn 


. 0, n E,„ n 0, n 
Ka=f(xa,Ua) = | “ “ + | e Im (4.1) 


Für den weiteren Verlauf des Kapitels wird für eine bessere Lesbarkeit auf den Index 
din den gewünschten Zustandsgrößen und Eingangsgrößen verzichtet. Die Trajektorie 
vom Anfangszustand xo zum Endzustand xy mit einer Zeitdauer T' wird durch N+1 
Stützstellen dargestellt. Die Werte an den Stützstellen k € {0,N — 1} zu den Zeitpunkten 
t = kAt und der zeitlichen Schrittweite zwischen den Stützstellen At=7,=T/N werden 
über die sog. Trapez-Methode [19] 


1 
X = Rp + zAtlEr + fer) (4.2) 


mit , =f(x=x7,u= u;,) aus (4.1) berechnet. Die optimalen Werte der Zustandsgrößen 
x7, und der Eingangsgrößen u), an den Stützstellen berechnen sich aus dem beschränkten 
statischen Optimierungsproblem 


N 
eu J(&)=T+At 3 ulu; (4.3a) 

k=1 

1 

u.Bv. xXk41 - X = „At (fr + fx+1) (4.3b) 
Xo=XS, XN=XT (4.3c) 
zer hell N (4.3d) 
um, <u kei N (4.3e) 
mit dem Vektor der Optimierungsvariablen €T = 4 x; us a ür, ae ‚un und den 


Beschränkungen der Zustände und der Eingänge x, X, u und u. In den Abschnitten 
4.2 und 4.3 werden insgesamt vier beschränkte statische Optimierungsprobleme der 
Form (4.3) gelöst. Bei zwei Trajektorien ist jedoch eine gewünschte Trajektoriendauer 74 
vorgegeben. Durch das Entfernen der Trajektoriendauer 7 aus (4.3a) und im Vektor der 
Optimierungsvariablen € kann eine gewünschte Trajektoriendauer 7 = 7, mit At = Ta 


vorgegeben werden. 
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(Otte) 


Abbildung 4.2: Beispielanordnung der Koordinatensysteme des Objekts (0.20%020), des 
Greifpunktes (0,%gYg2g), der Hand (On&nyn2n) und der Kamera (0.x.Yyc2e): 
Die kleinen unbeschrifteten Koordinatensysteme stellen die Koordinaten- 
systeme (ty), i € {0,...,7} des Roboters aus Abbildung 2.1 dar. 
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4.1 Optimaler Griff 


Bevor die einzelnen Trajektorien des Roboters und der Finger berechnet werden können, 
muss die Pose der Hand zu Beginn und am Ende des Griffs zu den Zeitpunkten t = T 
bzw. t= T} + Ta bekannt sein. Hierfür wird in Abschnitt 4.1.1 die Pose des Greifpunkts 
definiert. Der Greifwinkel wird dabei auf Basis eines Optimierungsproblems berechnet. In 
Abschnitt 4.1.2 wird zuerst mit dem Greifwinkel die optimale Position und Orientierung 
des Handkoordinatensystems (O,tnYyn2n) zu Beginn des Griffs berechnet und anschließend 
die optimalen Zustandsgrößen der Finger q) bzw. qc am Ende des Griffs. Für die folgenden 
Berechnungen wird angenommen, dass sich das Greifobjekt zwischen den Fingern befindet, 
und somit die Greifpunkte gam Objekt und G zwischen den Fingern an der selben Position 
befinden. 


4.1.1 Optimaler Greifwinkel 


Um einen Griff zu planen, wird am Objekt aus Abbildung 2.4 ein Greifpunkt, gekenn- 
zeichnet durch das Greifkoordinatensystem (O,2g%Yg2g), definiert. Die Rotationsmatrix des 
Koordinatensystems des Greifpunkts in Bezug auf das Koordinatensystem des Objekts 
(0.%0Y020), siehe Abbildung 4.3, ist 


-c8d,g 0 -sindy 
RZ =RZ(d,) = RR, -zRy,s, = u 2 _ = Ös|; (4.4) 


wobei der Winkel #, im nächsten Schritt über ein Optimierungsproblem berechnet wird. 
Dies ermöglicht es den Abstand zwischen dem Endeffektor des Roboters und der Ebene, 
auf welche das Objekt platziert wird, zu Beginn des Greifvorgangs zu maximieren. 

Die Orientierung der Greifhand R# zu Beginn des Griffs ist durch die Orientierung des 
Greifpunktes RZ gegeben, dabei sind die x7-z7-Ebene und die x,-2g-Ebene deckungsgleich. 
Des Weiteren wird gefordert, dass die z7,- und z,-Achse parallel sind und in die gleiche 
Richtung zeigen. Der Winkel &, soll nun so festgelegt werden, dass der Abstand zwischen 
der x-y-Ebene und dem Ursprung des Roboterendeffektors maximal wird. Dadurch soll 
erreicht werden, dass weder die Kamera, noch die Linse sowie die Greifhand während des 
Greifvorgangs beschädigt werden. Äquivalent dazu ist das Maximieren der Länge s z,g AUS 
Abbildung 4.3. 

Die Länge s,,, berechnet sich mit dem Richtungsvektor e,,, der z-Achse des Koordina- 
tensystems (Oy&gYg2g) in Bezug zum Basiskoordinatensystem 


2,9 = 82,9 (g) = RöRöe:. (4.5) 


zu 
> ee (4.6) 


Da die Vektoren in (4.6) in entgegengesetzte Richtungen zeigen, ergibt sich für die Länge 
S;,g ein negativer Wert, was für die weitere Berechnung aber nicht weiter stört. Schließlich 
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lässt sich das beschränkte Optimierungsproblem 


min J(d,) = 52,9 (4.7a) 

g 

uBr. nee, (4.7b) 
bg < bg < bg, (4.7c) 


für die Berechnung von d, mit der unteren und oberen Grenze d, = 0° und d, = 110° 
formulieren. Die Grenzen für den zulässigen Bereich (4.7c) werden so festgelegt, damit 
keine Kollisionen zwischen den Fingern und dem Objekt auftreten. 


Abbildung 4.3: Seitenansicht des Greifobjekts. 


4.1.2 Finaler Greifzustand 


Ausgehend von der Lösung zu (4.7) werden nun die neu eingeführten Zustandsgrößen des 
Griffs qc,n = [de,.7, PT» YVGT zen] zum gewünschten Endzeitpunkt 73 berechnet. 
Über die inverse Kinematik (2.37) lassen sich die Zustandsgrößen der Finger qı,r, angeben. 
Das Greifen mit den Fingern wird zum Zeitpunkt t= T gestartet und ist zum Zeitpunkt 
t= T, + T3 abgeschlossen. Wie im vorigen Abschnitt beschrieben, sind zu Beginn des 
Greifvorgangs mit den Fingern zum Zeitpunkt = T1 die Achsen 27, und 2, parallel und 
zeigen in die gleiche Richtung. Ein Greifen des Objekts ist ebenfalls möglich, wenn die 
Greifhand um 180° um die z,-Achse rotiert wird. Zunächst wird eine Bedingung formuliert, 
wann eine solche Rotation erforderlich ist. Darauf aufbauend kann im Anschluss der 
optimale Endzustand qn,r, bzw. qG,r, der Finger nach dem Griff berechnet werden. 

Im ersten Schritt wird ein zusätzliches Koordinatensystem (05254525) festgelegt, wel- 
ches um den Winkel m um die z,-Achse des Koordinatensystems (0,24Yg2,) gedreht ist, 
entsprechend 

R3= RER, (4.8) 


Der Ursprung der beiden Koordinatensysteme (O,24Yg2,g) und (05254525) befindet sich an 


der Position | 
d) — d) =dh + Rod. (4.9) 
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Zu Beginn des Griffs sind die Greifpunkte g bzw. 9 und der Greifpunkt G der Hand aus 
Abbildung 2.7 deckungsgleich. Mit der Annahme, dass die x,-Achse parallel zu den Achsen 
x, bzw. x; ist und jeweils in die gleiche Richtung zeigt, ergeben sich daraus die zwei 
möglichen Positionen und Rotationen der Hand mit der Vorwärtskinematik (2.25) zu 


RO =RZ=RÖRS, d=di-Ripf, (4.10a) 
RE=RI=RERS, d=d-Ripf. (4.106) 
Ausgehend von (4.10) ergeben sich mit R£, R#, d$ und d# aus Tabelle A.1, und 
RE = (RA) RR, d’=-di+dt (4.11) 
die möglichen Positionen und Rotationen des Kamerakoordinatensystems 
R5=R% (R) = dt _.Ridk, (4.12a) 
Rö=RL(RR) , &=di —Rödk. (4.12b) 


Die x-Achse des Greifpunktkoordinatensystems wird nun so festgelegt, dass zwischen den 
beiden Möglichkeiten jene mit dem maximalen Abstand des Kamerakoordinatensystems 
zum Koordinatensystem der Objektauflagefläche gewählt wird. Damit ergibt sich 
Ri di ; >; 
mh. mu ann il 
Am Ende des Griffs ist das Objekt tangential zur Greiflläche der beiden Finger. Dadurch 


ist das Koordinatensystem der Hand (O,&nyn2n) um den Winkel -yan = -pelt=T) 
um die zn-Achse gedreht, entsprechend 


Ron, R) (t T) R) (t T1) R.,-pc,15: (4.14) 


g wenn IIdölle ” Idölls (4 13) 
g sonst. 


Der Vektor phr, vom Greifpunkt G zum Koordinatensystem (O,C„Yn2n) im ortsfesten Ko- 
ordinatensystem (Oy&nYyn2zn) ergibt sich im Basiskoordinatensystem (Oxyz), gekennzeichnet 
durch den linken Index 0, zum Zeitpunkt t= 7] + Ta aus 


Bin = -RönPpk (gc.n)- (4.15) 


Ähnlich zur Berechnung des optimalen Greifwinkels &, in Abschnitt 4.1.1 können aus der 
z-Komponente des Vektors Dh: gegeben durch die Projektion auf die z-Achse 


Sch e/ den, (4.16) 


zwei weitere Optimierungsprobleme formuliert werden. Deren Lösung führt jeweils auf 
optimale Greifzustände, die notwendig sind um das Objekt aufzunehmen. Die Optimie- 
rungsprobleme lauten schließlich 


an J (4G,7) = +32.0n (4.17a) 
uBy =. H<0 (4.17b) 
hc (q6,n) < 0 (4.17c) 
9G S 96, S 4G, (4.17d) 
da,n = 0, (4.17e) 
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mit dem zulässigen Gebiet ha (qG,7,) gemäß (2.46) und den Beschränkungen qa = 


de,g ale zc . bzw. dc = [da,96,96: ze] Über das Vorzeichen der Kostenfunktion 
J (q4G,r,) wird die Richtung der Rotation der Greifhand um die x,-Achse am Ende des 
Griffs definiert, womit sich je nach Position und Orientierung des Greifobjekts flüssigere 
Greifbewegungen realisieren lassen. Die einzelnen Werte der Beschränkungen sind der 
Tabelle A.8 im Anhang zu entnehmen. Durch die Ungleichungsbeschränkung (4.17b) wird 
sichergestellt, dass die Distanz (4.16) stets positiv bleibt. Damit ist eine Bedingung für 
die minimale Steigung der z7-Achse in Bezug zur x-y-Ebene des Basiskoordinatensys- 
tems gegeben. Die Gleichungsbeschränkung (4.17e) gibt an, dass die Finger am Ende 
vom Griff geschlossen sind. Die optimalen Zustandsgrößen der Finger am Ende vom 
Griff q,, 7, ergeben sich schließlich aus der inversen Kinematik (2.37). Durch die Unglei- 
chungsbeschränkungen (4.17c) ist sichergestellt, dass die Lösung der inversen Kinematik 
existiert. 

In diesem Abschnitt wurden die optimalen Zustandsgrößen der Finger am Ende des 
Griffs zum Zeitpunkt t= T} + Ta berechnet. Des Weiteren ist über den Initialzustand der 
Finger qn,0 und der optimalen Lösung q7, 7, mit (4.10a) und (4.13) die optimale Pose der 
Greifhand HA(t =Tı) zu Beginn des Greifens zum Zeitpunkt t = T} sowie die optimale 
Pose der Greifhand H%(t = T, + Th) am Ende des Griffs definiert. Darauf aufbauend wird 
in den nächsten Abschnitten die Trajektorie der Finger sowie des Roboters berechnet. 


4.2 Greiftrajektorie der Finger 


In diesem Abschnitt wird die Trajektorie für das Greifen des Objekts mit den Fingern 
berechnet. Als Zeitdauer für das Schließen der Finger wird t= T5 = 0,55 festgelegt. Eine 
Trajektorie für einen Griff kann in Gelenkskoordinaten q„ (t) geplant werden, oder aber 
auch in den Greifkoordinaten q<c (t) und über die inverse Kinematik in eine Trajektorie der 
Gelenkskoordinaten umgerechnet werden. Für beide Varianten müssen die finalen optimalen 


T 
Greifkoordinaten q4.n, = de, Yan Yet &&.n| bekannt sein. Im Folgenden ist die 
Planung der Greiftrajektorie direkt in den Gelenkskoordinaten angegeben. 
Ausgangspunkt für die Berechnung der Trajektorie in Gelenkskoordinaten ist äquivalent 
zu (4.1) das lineare, zeitinvariante zeitkontinuierliche Integratorsystem in Systemdarstel- 


en nn un (4.18) 
Erx7 


mit dem Zustand x — la, an e R!* und dem Eingang y = ER". 


lung 
07x7 Erx ] n 


Hehe) Be O4 


Über eine Darstellung der Trajektorie durch Na + 1 Stützstellen mit der zeitlichen 
Schrittweite bzw. Abtastzeit At, = u und der Trapez-Methode (4.2) ergibt sich für die 
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Berechnung der Trajektorie ein beschränktes statisches Optimierungsproblem, gemäß 


Na 
min Ji&,)= At, ’2 UN Funk (4.19a) 
En En - 
1 

u.B.v. Xh,k+1 — Xh,k = 5Atn (fr r + fn,x+1) (4.19b) 

Yan, — YET. 
YG,k+1 — YG,k = Zn Ps - (4.19c) 

G 

%n0=XnSı Xh,Ng = Xh,E (4.19d) 
X <Kpk<Im k=0,...,Ng (4.19e) 
Un S Un,k <wm, k=0,...,NG (4.19£) 
mit fnr = fr(&n = Xnk,Un = Unx), den Vektor der Optimierungsvariablen Er — 
A an ee urnd| ‚ dem Anfangszustand Xıs — [af0,07], dem Endzu- 


T 
stand x E= (di) 0") aus Abschnitt 4.1.2, den Beschränkungen der Zustände x7 


und x, und den Beschränkungen der Eingänge ur und u7,. Durch die Gleichungsbeschrän- 
kung (4.19c) bewegt sich der Greifpunkt G mit einer gleichmäßigen Geschwindigkeit vom 
Anfangswert yc,r, zum optimalen Endwert y4 7,. Durch das Lösen des Optimierungspro- 
blems (4.19) ergeben sich die optimalen Werte der Stützpunkte £}. 

Es wurde die Planung der Greiftrajektorie in den Gelenkskoordinaten q„ sowie in den 
Greifkoordinaten qc implementiert und getestet. Formal lässt sich das Optimierungspro- 
blem für die Berechnung der Trajektorie in den Greifkoordinaten äquivalent zu (4.19) 
angeben. Die Implementierung hat aber gezeigt, dass der verwendete Lösungsalgorithmus 
für das Optimierungsproblem aufgrund der Schrittweite in das nicht zulässige Gebiet 
springt, wodurch der Algorithmus nicht terminiert und keine Lösung gefunden wurde. 


4.3 Roboterbewegung 


In diesem Abschnitt wird die Trajektorie des Roboters berechnet. Die gesamte Bewegung 
des Roboters setzt sich aus drei einzelnen Trajektorien zusammen. Um Sprünge zu 
vermeiden, wird der Endzustand einer Trajektorie als Anfangszustand der folgenden 
Trajektorie festgelegt. In Abschnitt 4.3.1 wird die Trajektorie für die Bewegung des 
Roboters im roten Bereich der Zeitlinie in Abbildung 4.1 während des Greifens des 
Objekts geplant. Daraus ergeben sich die optimalen Zustandsgrößen des Roboters q 
und q zu den Zeitpunkten t = Tı und t= T\ + T3, mit welchen in Abschnitt 4.3.2 die 
Robotertrajektorie von der Initialposition zum Objekt im blauen Bereich der Zeitlinie, 
sowie im Abschnitt 4.3.3 die Trajektorie für die Bewegung zur Zielposition nach dem 
Greifen des Objekts im grünen Bereich der Zeitlinie geplant wird. Im Folgenden werden die 
Optimierungsprobleme formuliert, mit denen die drei Trajektorien berechnet werden. Die 
Formulierungen beruhen auf (4.3), wobei für die Berechnungen der jeweiligen optimalen 
Trajektorien zusätzliche Ungleichungsbedingungen definiert und berücksichtigt werden. 
Ausgangspunkt für die Berechnung der Trajektorien des blauen Zeitbereichs mit i=1, 
des roten Zeitbereichs mit ö = 2 und des grünen Zeitbereichs mit ö = 3 ist äquivalent zu 
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(4.1) ein lineares, zeitinvariantes zeitkontinuierliches Integratorsystem 


07x7 
+ U; 4.20 
Ba ' en 


mit dem Zustand des Roboters x! = la", a") e R!* und dem Eingang u; =geR’. 


nn 1 |O7x7 Erx7 
een — Orxr |” 


4.3.1 Schwenkbewegung während des Greifens 


Das Ziel des dynamischen Greifvorgangs des Objektes ist es eine möglichst schnelle, 
flüssige und zuverlässige Bewegung auszuführen. Die geplante optimale Trajektorie soll 
also sicherstellen, dass während des Greifvorgangs der Roboter nicht zum Stillstand kommt. 
Die Geschwindigkeiten q des Roboters am Beginn und am Ende des Griffs sind noch 
nicht definiert, weshalb zuerst die Robotertrajektorie während des Greifens des Objekts 
berechnet wird. Beim Schließen der Finger liegen die Greifpunkte G der Finger und g am 
Objekt übereinander. Es wird angenommen, dass die Normale der Verbindungslinie C} und 
C3 im Punkt G, siehe Abbildung 2.7, und die z,-Achse während des Schließens der Finger 
annähernd parallel sind. Das Objekt liegt somit immer tangential zur Greiflläche der Finger. 
Daraus ergibt sich, abhängig vom optimalen Endwert Yan: eine Schwenkbewegung der 
Greifhand, welche über die in diesem Abschnitt berechnete Trajektorie des Roboters 
durchgeführt wird. 

Die Position I = d{ (q.) des Greifpunktes G im Basiskoordinatensystem an den 
Stützpunkten der Trajektorie ergibt sich aus der Vorwärtskinematik (2.4b) und (2.25) zu 


d6.r = do (gr) + Rü (AR) Ph (ne): (4.21) 
Da die Punkte G und g übereinander liegen, sind die Positionen auch über 
d,=d (4.22) 


gegeben. Der Vektor d bildet sich mit der Schätzung des Kalman-Filters aus (4.9). Für 
die Trajektorie muss (4.22) nicht exakt erfüllt sein, sondern es werden kleine Abweichungen 
zugelassen. Daraus ergeben sich die Beschränkungen 


Ads < de, - dd < AdS (4.23) 


mit den unteren und oberen Grenzen Ad, und Ady. Die experimentell gewählten Grenzen 
sind der Tabelle A.8 im Anhang zu entnehmen. Dadurch wird der Lösungsraum des 
Optimierungsproblems erweitert und die Erfolgsrate erhöht, weil der Roboter eine größere 
Bewegungsfreiheit hat. 

Durch die in Abschnitt 4.2 berechnete Trajektorie der Finger ist der Winkel Par ZU 
jedem Zeitpunkt k bekannt und dadurch auch die Position des Koordinatensystems der 
Hand 


Rö,x = RöRz.-g;,,; (4.24a) 


dn = dh - Rüıpr (air) (4.24b) 
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bzw. über die Vorwärtskinematik 
dr= (ar): (4.25) 


Äquivalent zu (4.23) werden für (4.25) kleine Abweichungen 


Ady < do, - di (ar) < Adk (4.26) 


mit den unteren und oberen Grenzen Adh und Adh zugelassen. Die experimentell ge- 
wählten Grenzen für eine größere Bewegungsfreiheit des Roboters sind in Tabelle A.8 im 
Anhang angegeben. 

Die Geschwindigkeit des Koordinatensystems der Hand im Basiskoordinatensystem 
berechnen sich in jedem Schritt über die Beziehung (2.6) und (2.7) zu 


vr : 
I = Ih (9r) &k; (4.27) 
0,k 


Für die lineare Geschwindigkeit va „ am Ende vom Griff zum Zeitpunkt k = Ng wird nun 
gefordert, dass die z-Komponente nicht negativ ist, entsprechend 


evon. 20, (4.28) 


oder äquivalent 
-elvong <d. (4.29) 


Dadurch wird gewährleistet, dass das Objekt von der Ebene abgehoben wird. 
Die Trajektorie bildet sich wie in (4.19) aus Ng + 1 Stützstellen mit der Schrittweite 
At = ne Die optimalen Werte der Stützpunkte der zweiten Trajektorie x}, und u} , für 


k=0,1,...,Nc errechnen sich schließlich aus dem beschränkten statischen Optimierungs- 
problem 
N 
min. JilE,)= Ab > UL Ug% (4.308) 
& k=1 ’ 
1 
u.B.v. X2,k+1 = X2,k = „At (fr. + f.+1) (4.30b 
g0 = 982, AN. = qr2 (4.30c 
-e, von, <0 (4.30d 


Ag <d,- <A, Bela, Ne 


Ad; <din- lm) <Adk, k=0,...,Ne 4.30f 
x< X2,k <X, k= 0, ‚Ne (4.308 
u<s U2,k < u, k= ’ ‚Ne (4.30h 


mit der Trapez-Methode (4.30b) und f = f(x2 = x2,K,uU2 = u2,,) aus (4.20), dem Vektor 


der Optimierungsvariablen & — [x3o, Re > Na’ u ie ‚us Ne| ‚ den Beschränkungen 
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der Zustandsgrößen x und X, und den zusätzlichen Beschränkungen (4.23), (4.26) und 
(4.29). Die Anfangszustände gg» und Endzustände qra werden aus der inversen Kinematik 
von (2.4b) und (4.24) mit k € {0,Nc} berechnet. Es existieren keine expliziten Werte der 
Beschränkungen der Eingangsgrößen u und u in (4.30h). Es lassen sich jedoch aus den 
Bewegungsgleichungen (2.11) mit 4. = ua, und den unteren und oberen Grenzen des 
Drehmoments 7 und T des Roboters die Bedingungen 


formulieren und (4.30h) mit dieser ersetzen. Bei der Lösung der Optimierungsaufgabe (4.30) 
wird durch die aufwendige Berechnung von C (9x,9x) 4x in (4.31) die Laufzeit erheblich 
verlängert. Damit der Einfluss von © (q,.,9x) 4x auf die Drehmomente abgeschätzt werden 
kann, wird im Vorfeld eine Monte-Carlo-Analyse mit 10! gleichverteilten Zufallsvariablen 
g, € aa] und g. € aa durchgeführt. Aus der Monte-Carlo-Analyse ergibt sich die 
Abschätzung 

C<C (4x) Ar <C. (4.32) 


Zusammen mit (4.31) ergeben sich schließlich die Ungleichungsbeschränkungen 
T-C<D(y)wıt+ga)<r-C, k=0,...,NG (4.33) 


welche die Bedingungen (4.30h) ersetzen. Damit wird sichergestellt, dass die Beschrän- 
kungen des Roboters eingehalten werden. Die einzelnen Werte der Beschränkungen 
x’ = lat,a”], xt - a’), T, 7, C und C sind der Tabelle A.8 im Anhang zu 
entnehmen. 

Die Zustände x3,, und x5 „, aus der optimalen Lösung &5 sind jeweils die Anfangs- 
oder Endzustände für die Trajektorien der folgenden zwei Kapitel. 


4.3.2 Von der Initialposition zum Objekt 


In diesem Abschnitt wird die Robotertrajektorie im blauen Bereich der Zeitlinie aus 
Abbildung 4.1 berechnet. Dabei bewegt sich der Roboter vom Initialzustand xo zur 
Anfangsposition des roten Teilabschnitts der Zeitlinie, an dem das Zugreifen mit den 
Fingern gestartet wird. Die Trajektorie der Roboterzustände wird über ein weiteres 
statisches beschränktes Optimierungsproblem gemäß (4.3) berechnet. Um möglichst viele 
Bilder für die Poseschätzung des Objekts während der Roboterbewegung zur Verfügung 
und unerwünschte Kollisionen der Finger mit dem Objekt zu vermeiden, werden zusätzliche 
Beschränkungen für das statische beschränkte Optimierungsproblem formuliert. 
Während der Greifbewegung ist es notwendig, zyklisch neue Messungen für das Update 
der Posenschätzung durch das Kalman-Filter zu erhalten. Dafür muss gewährleistet 
werden, dass sich das Objekt stets im Sichtfeld der Kamera befindet. Dazu wird in jedem 
Abtastschritt k über (3.3)-(3.5) der Ursprung des Objektkoordinatensystems (0.%0Yo2o) 


aus der Sicht des Kamerakoordinatensystems (0.x.Yy.2.) in den Punkt Di. „in die Bildebene 
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projiziert, gemäß 


J 0 & 

=0 % % [R2.ld2.| (4.34a) 
00 1 
if 

= — A (4.34b) 
Wr | Ur 


Es wird angenommen, dass die geschätzte Pose des Objekts mit RS und ds konstant ist. 
Die Rotation R}, und Translation d}, zwischen dem Kamerakoordinatensystem und dem 
Objektkoordinatensystem ergibt sich damit in jedem Abtastschritt k zu 


H3,= (Hi (q,)) A (4.35) 


mit der homogenen Transformation (2.4). Das Kamerabild hat eine Auflösung von 1024 x 
1024 Pixel, woraus sich die Beschränkungen 


Dep, (4.36) 


für das Objektkoordinatensystem in der Bildebene ergeben. 

Durch die schnelle Bewegung der Kamera kann es dazu kommen, dass die Bilder 
aufgrund der Bewegungsunschärfe unbrauchbar für die Schätzung der Pose des Objekts 
werden. Damit solche unscharfen Bilder durch die schnelle Kameraführung vermieden 
werden, wird die Änderung des Punktes 2 zwischen den einzelnen Schritten k durch 


AP. = Des = Der Ap} (4.37) 


begrenzt. Die oberen und unteren Grenzen P2, B2, Ap2 sowie Ap° sind in der Tabelle 
A.8 des Anhangs zu entnehmen. . . 

Um Kollisionen zu vermeiden und gleichzeitig die Richtung zu definieren, aus der sich 
der Greifpunkt der Finger G dem Greifpunkt am Objekt g annähert, wird die invertierte 
und verschobene zweidimensionale Gaußglocke 


hz, 1 T . BE T 
lee > () 


definiert. Dabei bezeichnet h,,o den Skalierungsfaktor, & die Kovarianzmatrix und 1 den 
Vektor der Erwartungswerte der Glockenkurve 


2 
hz,o = 2627 \/ det (2), = B A D HZ 


Die Höhe der Kurve lässt sich mit zc einstellen. Die Funktion fe (z,y) € [O,zG] aus 


(4.38) mit zG = 0,2m ist in Abbildung 4.4 dargestellt. Durch die Varianzen 0? und 
0, wird die Breite der Gauß-Glocke in x- und y-Richtung verändert. Die Varianzen 


wurden zu o2 =5mm und 0, = 2mm gewählt. Das Koordinatensystem der Gauß-Glocke 


0 
) (4.39) 
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Abbildung 4.4: Zweidimensionale Gauß-Glocke mit zG = 0,2m. 


N (te) - [te) 
o = o Q 
o o 


Je 2) 


Yayloııgıg ualM NL Te uud uı sıgejrene sı sısayı sıy) Jo uolsion jeuißuo panoldde ayıL 
Jeqbnyen Yayloıgıg ualıM NL Jap ue Isı Naqlewojdıq Jasaıp uoIsianfeulsllo amanıpaß auaıgoldde sıq 
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(OactaccyGGzcg) wird durch die Rotation R@ und die Verschiebung d$’“ mit 
Bo — RöR.,„Rz,,r; dy“ = dh (4.40) 


in den Greifpunkt g des Objektes gelegt. Durch die Rotation RS sind die z-Achsen 
der Koordinatensysteme (04X%9Yg2g) des Greifpunktes und (OaczecyagzGg) der Gauß- 
Glocke deckungsgleich, jedoch entgegengesetzt orientiert. Durch die invertierte und in den 
Ursprung des Greifpunktes verschobene Gauß-Glocke aus (4.38) wird nun eine Fläche 
definiert. Für die gesamte Trajektorie im blauen Bereich der Zeitlinie wird nun gefordert, 
dass der Greifpunkt der Finger G stets über der Fläche der Gauß-Glocke bleibt. Dafür 
wird zunächst im Basiskoordinatensystem mit (4.21) der Vektor vom Ursprung des Koordi- 
natensystems der Gauß-Glocke zum Greifpunkt G gebildet und in das Koordinatensystem 
der Gauß-Glocke transformiert 


2GG-G (9,9R) T 
P&c (aan) = | voc-6 (Gan)| = (RE) (a + (d+RE(A PL (An)): (4-41) 
2GG-G (4;4R) 


Dort lässt sich nun mit (4.38) und (4.41) die Distanz 


52,G-GG (9;9r) = 26G-G (9,9r) — Fa (20a-c (4,4R) »YGG-G (A,9r)) (4.42) 


definieren, die den Abstand zwischen der Fläche der Gauß-Kurve und dem Greifpunkt der 
Finger angibt. Die Beschränkung zur Vermeidung von Kollisionen lautet schließlich 


— 52,0-6G (4,9) < 0. (4.43) 


Abbildung 4.5 zeigt ein Beispiel der Entfernung s, GGG, in dem sich der Greifpunkt der 
Finger G auf der yy-zg-Ebene in der näheren Umgebung des Greifpunktes g befindet. Die 
Schnittkurve fc (0,y,) der Gauß-Glocke mit der yg-24-Ebene ist ebenfalls eingezeichnet. 

Die ersten Trajektorie bildet sich aus N Stützstellen mit der Schrittweite At = A. Die 
optimalen Werte der Stützpunkte x] , und uj , für k=0,1...N der ersten Trajektorie 
sowie die optimale Trajektoriendauer 77 ergeben sich schließlich aus dem beschränkten 
statischen Optimierungsproblem 


N 
2 J(£&)=Tı+At >> Ulruk (4.44a) 
1 k=1 
1 
u.B.v. X1,k+1 = X1ık = „At (fj + f.+1) (4.44b 
x1,0=Xsı, N =XTi (4.44c 
— 52,G-GG (9x ,4n.0) <0, k=0,...,N (4.44d 


4.44e 
4.44f 


Be<PBOL<PR, k=0,...,N 
AP < Perrı - Don < ABS, k=0,...,N-1 
XSRIRSK, k=0,...,N 


4.44g 
rT-C<Dig)we+ts(a)<tTr-C, k=0,....,N 


( 
( 


( 
(4.44h 


) 
) 
) 
) 
) 
) 
T ) 
Men (4.44i) 
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Sc (0,49) 


52,G-GG 


(OgrgYg2g) 


> 
S——i-> 


\ 


Abbildung 4.5: Seitenansicht des Greifobjekts und der Fingerspitzen mit eingezeichneter 
Begrenzungskurve fc (0,y0) durch die Gauß-Glocke. 
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mit der Trapez-Methode (4.44b) und f = f(xı = x1,K,Uı = u1,,) aus (4.20), dem Vektor 
der Optimierungsvariablen e’ = 7, x1o nad wo N ulo u ‚un N ‚ dem Anfangszustand 


x, — [ad ,07 ‚ dem Endzustand xrı = x, aus der Lösung &5 von (4.30), und den 


zusätzlichen Beschränkungen (4.36), (4.37) und (4.43). Die Werte aller Beschränkungen 
sind in Tabelle A.8 angegeben. 


4.3.3 Zur Endposition nach dem Greifen 


Die dritte und letzte Trajektorie im grünen Bereich der Zeitlinie in Abbildung 4.1 be- 
schreibt die Bewegung des Roboters vom Ende des Griffs zur Zielposition. Für die gesamte 
Trajektorie wird gefordert, dass in jedem Abtastschritt k die z-Komponente der translato- 
rischen Geschwindigkeit des Endeffektors vL „ aus (4.27) nicht negativ ist, woraus sich die 
Beschränkung 

-elvg,<0 (4.45) 


zZ 


ergibt. Damit wird erreicht, dass sich das Objekt nicht mehr auf die Ebene zubewegt, auf 
der es am Anfang gelegen ist. 

Die dritte Trajektorie bildet sich äquivalent zu (4.44) aus N Stützstellen mit der 
Schrittweite At = 2. Die optimalen Werte der Stützpunkte x3 , und u; , für k=0,1...N 
der dritten Trajektorie sowie die optimale Trajektoriendauer 73 ergeben sich schließlich 
aus dem beschränkten statischen Optimierungsproblem 


N 
2 J(£3) =T3 + At 3 UI KUS,% (4.46a) 
5 k=1 
1 
u.B.v. X3,k+1 - X3k = „At (f: + fr+1) (4.46b 
X30=X3, X3 N =XT3 (4.46c 
-elvga<0, k=0,...,N (4.46d 


( 
r-C<D(q)ur+g(n)<rTr-C, k=0,...,N (4.46f 


) 
) 
) 
REBEER, Bel, N 4.46e) 
) 
T3<T3<Tz (4.468) 


mit der Trapez-Methode (4.46b) und fj, = f(x3 = x3 ,,U3 = ug ,) aus (4.20), dem Vektor 
der Optimierungsvariablen £! = 7, x30 Br RI u3o Be ‚ug ‚ dem Anfangszustand 
xs3 =xX5 nn. aus der Lösung E5 von (4.30), dem Endzustand 2, — [47,07], und der 


zusätzlichen Beschränkung (4.45). Die Werte aller Beschränkungen sind in Tabelle A.8 
angegeben. 


4.4 Interpolation der Stützpunkte 


In diesem Abschnitt wird eine Interpolation der Stützpunkte der Trajektorien aus Ab- 
schnitt 4.2 und 4.3 durchgeführt. Eine Interpolation ist notwendig, da für die Regelung 
eine wesentlich feinere Auflösung benötigt wird. Von einer Vergrößerung der Anzahl der 
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Optimierungsvariablen ist abzuraten, da damit die Rechenzeit stark ansteigen würde. 
Für jeden einzelnen Zeitbereich aus Abbildung 4.1 werden die zugehörigen optimalen 
Stützpunkte der Trajektorie &,i € {1,2,3} sowie €), über das jeweilige zugehörige Zeitin- 
tervall te [0,T}], t € [T1,7ı + Ta] bzw. t € [Tı + 72,7] mit so genannten B-Spline Kurven 
interpoliert. Somit wird für jede der vier Trajektorien der einzelnen Zeitbereiche getrennt 
eine Interpolation durchgeführt. Die Implementierung der B-Spline-Interpolation wird 
anhand von [20] durchgeführt. 

Eine B-Spline-Kurve mit vorgegebenen Ableitungen an den Rändern Cxv (t) ‚t € [0,1] 
der Ordnung p, die jeweils durch die N bzw. Ng Stützstellen in &},i € {1,2,3} bzw. &7, 
der Kapitel 4.2 und 4.3 läuft, bildet sich über 


N+2 
Cl) 3, 0)P; (4.47) 
i=0 


mit den sog. Basisfunktionen B;, der Ordnung p und dem Kontrollpunktvektor P;,i € 
{0,...,N +2}. Die Ordnung p wird je nach gewünschter stetiger Differenzierbarkeit der 
Kurve (4.47) gewählt. Für eine r-fach stetig differenzierbare Kurve gilt für die Ordnung 


p2r+l. (4.48) 


In dieser Arbeit soll die Kurve zweimal stetig differenzierbar sein, also r = 2 und somit 
ergibt sich die Ordnung 
p=r+1=3. (4.49) 


Für die Berechnung der Basisfunktionen wird der Vektor der normierten Zeit der Trajek- 
torie t = [to,... ‚tn] mit 


to — 0, (4.50a) 
in=1, (4.50b) 
= D j 
B= on i=l...,N-1 (4.50c) 


gebildet. Zusätzlich wird der sog. Knotenvektor tx = [tx.o; Kl N+2)4p+1| definiert. 
Die Elemente tx,;,i € {0,...,N +p+3} von tx werden als Knoten bezeichnet und es 
gilt 

tKi SER; i=(0,...,‚N+p+2. (4.51) 


Die Elemente des Knotenvektors tx sind 


tKko='=tkp=0 (4.52a) 
IK,N+3 = = bK,N4p43 = 1 (4.52b) 
1 J+p-1 = 
IK, j+p+1 "» > bi, yalı, N=-p+l (4.52c) 
i=j 
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Mit den Elementen des Knotenvektors tx.,i € {0,...,N+p+3} sind die N Basisfunk- 
tionen B;, durch 


wenn tk, <St<t 
En (d) ’ Ki K,+l (4.53) 
0, sonst 
B de et 
u (t) = 5, Bip-ı (ft) + 5 — Birp-ı (f) (4.53b) 
IxK,itp Z— IK. IKitptl = ik. 


definiert. Die Basisfunktionen B;,; (t) ‚5 € {0,1,2,3} sind in Abbildung 4.6 dargestellt. Die 
Ordnung p bestimmt den Einfluss eines einzelnen Kontrollpunktes auf die Teilbereiche der 
Kurve. Für die Bereiche t < tx,; und t >= tKitprı gilt Bs)=V 


1.2 T 


ik IK,i+1 IK,i+2 IK .i+3 IK,i+a 
t 
Abbildung 4.6: B-Spline Basisfunktionen B;,; (t) ,j € {0,1,2,3} 


Die Ableitungen der B-Spline-Kurve an den Endpunkten sind in der Form 


Soxv ar _ En (PI<PRi), (4.54a) 
‚ 
on A| = — (4.54b) 
1<-t&,n42 


gegeben. 

Um die einzelnen Trajektorien durch eine B-Spline-Kurve nach (4.47) zu interpolieren, 
wird zunächst die Zeitachse der jeweiligen Trajektorie t auf die jeweilige Trajektoriendauer 
T=T,,i e {1,2,3} normiert, gemäß 


t = (4.55) 


t 
7 
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Das Argument der Kurve (4.47) wird nun durch die normierte Zeit ?’ ersetzt. Aus 
den N einzelnen Stützstellen der vorigen Kapitel bilden sich die Punktvektoren Qr = 


lar,af, uf] ‚ke0,...,N. Die Punkte Q,. berechnen sich aus der B-Spline-Kurve aus 


N+2 
Qr = Onv (tk) = I Bin (ti) Pi, (4.56) 
i=0 


wobei durch die Normierung der Zeit für die Zeitpunkte t, = tg, k € {0,...,N} gilt. Für 
die zeitliche Ableitungen Q] = [9r,äx,ü;] an den stellen k € {0,N} ergibt sich mit (4.54) 


t B 
Pipe =. Os: (4.57a) 


1-t 
Zr (4.57b) 


-Pxn+ı +Pn#2 


Aus den N Gleichungen (4.56) und den zwei Gleichungen (4.57) lässt sich das lineare 
Gleichungssystem 


BkPxk =Qk (4.58) 
mit 
T 
12:0 a AT 
p 
Pi an 
PT : 
Pr=| |, ax=| : (4.59) 
ON- 
Px42 1-t 
B ol 
T 
N 
und 
Bo,» (th) Bu (%) Bap(%) EB (6) Bn+1, (to) Bn+2, (to) 
=i 1 0 ... {0} 0) {0} 
Bo,p (4) Bi,p (4) Ba,p (4) = BN» (4) Bn+1,P (4) Bn+2,p ©) 
Bxk = i 
Bo,» (&r ;) Bi,p (sa) B2,p (ir) Bnp (!v-:) Bn+1,p (ia) Bn+2,p (=) 
Pe =4 
Bo. (tv) Bi» (ty) Bap(ty) Belt) Bn+1o (tr) Bn+2,» (tiv) 
60) 
formulieren, woraus die Kontrollpunktvektoren P;,i € {0,...,N +2} über 
Px = By Qk (4.61) 


berechnet werden. Die interpolierten Zustände q und q sowie die interpolierten Eingänge 
u werden schließlich über 


a) : N+2 
at)| =Cnv (‘ _ ) _ 2 B;,(£)P; (4.62) 
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berechnet. Die Zustandsgrößen x;(t) und Eingangsgrößen u;(t) für öi € {1,2,3,h} berechnen 
sich somit in jedem Zeitabschnitt t € [0,71], te [71,71 + T3] undt IT, +R,Tn+T7T+ T3] 
aus einer eigenen B-Spline-Kurve On, (t’) ,i € {1,2,3} des Roboters sowie Cnv,n (t) der 
Hand im Zeitabschnitt t € [T1,Tı + Ta], gemäß 


Cxrlt =D), wenn t<0 
q(t) Cnvi = +) ; wenn O<t<T 
a(t)| = $Cnva (t = Et) : wenn T, <t<TN +7 (4.63a) 
ut) Cnvs3 t = EIER), wm BB <s:<n+72+73 
Cnus(!=]), wnt>n"+nR+73 
a 
q, (t) [*7,,07] ; wenn t<T} 
HE| =! Canon (F - 4) mie Dt (4.63b) 
7 
un (t) [*7,2,0°] ; wennt>N+T7 


(4.63) 


4.5 Online Trajektorienupdate 


Die Genauigkeit der Posenschätzung während der Roboterbewegung wird mit sinkender 
Entfernung zwischen dem Objekt und der Kamera zunehmend besser. Wegen der sich 
ständig ändernden geschätzten Pose des Objekts ist ein Update der ursprünglich geplanten 
Trajektorie erforderlich. Da das Planen der Trajektorie über (4.44), (4.30) und (4.46) mehre- 
re Sekunden benötigt, ist diese Methode für ein Trajektorienupdate während der Bewegung 
nicht geeignet. Auf Basis von [10, 11] wird nicht die gesamte Trajektorie neu geplant. Die 
Planung erfolgt für die Abweichung ö&,; = [N öAt, 6x9, ... . ‚6x ,ÖUo,... ‚dun],i € {1,2,3} 
für jeden Teilabschnitt i € {1,2,3} unter der Annahme, dass sich die geschätzte Pose des 
Objektes zwischen den einzelnen Updates nur gering ändert. Anschließend wird für die 
upgedateten Trajektorien &; = €} + ö£&,,i € {1,2,3} erneut eine Interpolation nach Kapitel 
4.4 durchgeführt. 

Ausgangspunkt für die Formulierung zur Berechnung der Updates ist die Trapez-Methode 
(4.2). Einsetzen von x, = x7, + 6Xr, Up = ur + du, und At = At* + öAt ergibt 


At* + öAt 


5 (£ (x7. + öx,,ur + dur) + 


Krrı + 6%krı RE 08% = 
f (Xi + 6X, + FuR41))- (4.64) 


Die Schrittweite At” ist die Zeitdauer zwischen den einzelnen Stützstellen der betrachteten 
Trajektorie €/,i € {1,2,3}. Aus der Taylor-Formel und Vernachlässigen der Terme zweiter 
und höherer Ordnung von öx; und öu;, ergibt sich die linearisierte Systemdynamik zu 
öf öf 
f(x +5x.,u, +du.) = f(xuR) + — oe dux. (4.65) 


OX BE EEE. ou —_y* 1% 
x=x7 ,u=u,, x=x} ,u=u/, 
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Einsetzen der Taylor-Formel für f (x}. + öx,,uj, + dur.) bzw. f (Kr + OXk+ U H 1 + dug-) 
in (4.64) und vernachlässigen der Terme mit 6Atöx;,i € {k,k +1} und ÖAtöu,,i € 
Ik,k +1} ergibt 


At* Me 
ÖXk41 = 6x, + 5 (TE Ox,. + Tour + Tr; 410%kH1 + T},1dux+1) + Eu (fr + fi) 
(4.66) 
mit 
=), (4.672) 
1% — öf _ |Orx7 Erxr (4.67b) 
ae: x=xtu=us |[07x7 Orr 


„of 


für k = 0,...,N —1. N bzw. Na ist dabei die Anzahl der Stützstellen der jeweils 
betrachteten Teiltrajektorie. 

Die Abweichungen d&,; = ö&,i € {1,2,3} jeder Teiltrajektorie berechnen sich jeweils aus 
einem Optimierungsproblem der Form 


= Be (4.67c) 


Erx7 


_y* aM 
X=X, ,U=Uj, 


min J (ö€) = SöE"QE (4.68a) 

u.B.v. (4.66) (4.68b) 

6X =Rs X, Ö6XN=XT—-XN (4.68c) 

öx<öx, <öx, k=0,...,N (4.68d) 

öAt < 6At < ÖAt (4.68e) 
rT-C<D(g +) tin) tgl tig) <T-C, k=0,...,N 

(4.68f) 


mit den neuen Startzuständen Xg, den neuen Endzuständen Xr und der positiv definiten 
Gewichtungsmatrix Q = diag (QAt,Qxo- -- -Qxn Quo; - -- ‚Qu, )- Die unteren und oberen 
Grenzen sind über 6x = x — x* und 6x =X — x* gegeben. Durch die Grenzen in (4.68e) 
kann die maximale Änderung der Trajektoriendauer vorgegeben werden. Durch Versuche 
wurden sie zu dAt = 0,0s und öAt = 0,01s gewählt. Damit wird nur eine Verlängerung 
der Trajektoriendauer zugelassen. Durch die Beschränkungen (4.68f) bleibt die Summe aus 
dem bereits berechneten Momenten uj, und den optimalen Abweichungen du im zulässigen 
Bereich, wodurch die Beschränkungen für diese auch hier berücksichtigt werden. 


Für die erste Teiltrajektorie Uxv,ı (X — #) wird zyklisch in einer Schleife ein Opti- 


mierungsproblem der Form (4.68) gelöst. Das Update wird dabei nicht für den gesamten 
Zeitbereich t € [0,71] berechnet, sondern vom Startzeitpunkt des jeweiligen Schleifdurch- 
laufs tacı inklusive eines genügend großen Zeitoffsets At’ bis zum Ende T} der Trajektorie. 
Der Zeitbereich, für den ein Update durchgeführt wird, ist damit t € [tact + At’,T,]. Die 
Anfangszustände sind über 


ac A { 
u (4.69) 


Xs = Cnv,1 (r = 
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gegeben. Die neuen Gelenkswinkel qr, zum Endzeitpunkt ergeben sich mit dem Update 
der Pose H% des Objektes und (4.24) durch die Berechnung der inversen Roboterkinematik 
[21]. Diese wird numerisch mittels Gradientenmethode gelöst, wobei Oo, (? = 1)j1:7) als 
Startpunkt dient. Die Geschwindigkeiten g werden unverändert aus der Interpolation 
Cnv, (t =1) übernommen, womit der neue Zielzustand Xr festgelegt ist. Da sich der 
betrachtete Zeithorizont t € [tact + At’,T}] zunehmend verkleinert, müssen die optimalen 
Werte x}, sowie u}, für den Zeithorizont t € [tacı + At',T,] in N äquidistanten Zeitpunkten 
durch (4.63a) neu berechnet werden. Über eine weitere temporäre Interpolation der 
Lösung Ö&* aus (4.68) werden die einzelnen Werte der Abweichungen 6€7 im Zeitbereich 
t € [tact + At’,T,] berechnet. Die Werte in ö&] für den Zeitbereich t € [O,tacı + At] 
werden gleich Null gesetzt. Mit d&7 wird schließlich die erste Trajektorie C'xv,ı (t’) gemäß 
Abschnitt 4.4 neu interpoliert und somit ein Update durchgeführt. 

Zur Veranschaulichung dieser Vorgehensweise sind in Abbildung 4.7 die initiale Trajek- 
torie und die ersten drei upgedateten Trajektorien des Gelenkswinkels qı,; (t),i=0,1,... 
mit der Trajektoriendauer T} = 4s dargestellt. Die initiale Trajektorie qı,o entspricht 
der Lösung des Optimierungsproblems (4.44), welches vorab offline berechnet wird. Der 
Zeitoffset wird auf den konstanten Wert At’ = 0,55 gesetzt. Das Update der Trajektorie 
qı,0 startet zum Zeitpunkt ta. = 0,55. Somit wird für den Zeitbereich t € [1s,4s] über 
(4.68) die optimale Abweichung berechnet. Aus der initialen Trajektorie gı,o und der 
soeben berechneten optimalen Abweichung resultiert die Trajektorie qı,ı. Zum Zeitpunkt 
tact = 1,58 startet die Berechnung des Updates der Trajektorie gı,ı für den Zeitbereich 
te [2s,4s] und führt auf die Trajektorie qı,2. Über ein weiteres Update zum Zeitpunkt 
tact = 2,58 ergibt sich im Zeitbereich t € [3s,4s] aus der Trajektorie qı 2 die Trajektorie qı 3. 
Wie gezeigt, werden die Zeitintervalle t € [ta«t + At’,Tı] der einzelnen Trajektorienupdates 
mit steigendem Index i kürzer. 

Für die zweite Teiltrajektorie ändert sich der Anfangszustand, sowie durch die Bewegung 
der Finger und der Schwenkbewegung des Roboters auch der Endzustand. Aus dem neuen 
Endzustand der ersten Teiltrajektorie ergibt sich der Anfangszustand Xg und über (4.24) 
der neue Endzustand XT der zweiten Teiltrajektorie. Die Geschwindigkeiten g werden 
unverändert aus den Interpolationen Onv. (t' = 0) bzw. Onu,2 (t’ = 1) übernommen. Mit 
der optimalen Abweichung 6&5 = 6€* wird für die zweite Teiltrajektorie Onv.2(t’) ein 
Update durchgeführt. Das Update der zweiten Teiltrajektorie wird nur einmalig kurz vor 
dem Ende der ersten Teiltrajektorie berechnet. 

Zuletzt wird ein einmaliges Update der dritten Teiltrajektorie Oxu,z (t') mit 6€3 aus 
(4.68) durchgeführt. Der Anfangszustand Xg ist der Endzustand Cnv. (t’ = 1) der upgeda- 
teten zweiten Teiltrajektorie. Der Endzustand bleibt mit &v = Cnu; (t’ = 1) unverändert. 
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Abbildung 4.7: Exemplarische Darstellung der Initialtrajektorie qı,o und der Trajektori- 
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5 Trajektorienfolgeregler 


Die geplanten Trajektorien aus dem vorigen Kapitel werden vorgesteuert. Um etwaige 
Abweichungen gering zu halten bzw. zu eliminieren, wird in diesem Kapitel für den 
Roboter und die Finger ein Trajektorienfolgeregler entworfen. In Abschnitt 5.1 wird 
für den Roboter ein Computed-Torque Regler mit Integralanteil vorgestellt. Zusätzlich 
hat der Roboter unterlagert eine Regelung basierend auf der singulären Störtheorie 
und eine Reibungskompensation. Der Abschnitt 5.2 beschäftigt sich mit dem Entwurf 
des Trajektorienfolgereglers der Fingergelenke. Aufgrund der internen Steuerung bzw. 
Regelung der Greifhand wird der Trajektorienfolgeregler auf Basis einer Zwei-Freiheitsgrad- 
Regelkreisstruktur entworfen. Das Führungssignal für den internen Regler der Greifhand 
ist die gewünschte Geschwindigkeit 4n,a der Fingergelenke. Durch die Wahl einer Zwei- 
Freiheitsgrad-Regelkreisstruktur werden etwaige Abweichungen zwischen den gewünschten 
Gr,a und den tatsächlichen Zuständen qn berücksichtigt bzw. kompensiert. 


5.1 Roboter 


Für den Trajektorienfolgeregler des Roboters wird das sog. Regelgesetz der inversen 
Dynamik bzw. Computed-Torque mit Integralanteil [15] 


ra=D(q)v+C(qgd)ä+g(g) (5.1) 


mit dem neuen Stelleingang v verwendet. Wie bereits in Kapitel 4 erläutert, ist der 
Einfluss der Coriolis-Matrix gering und wird dadurch vernachlässigt. Das vorgegebene 
Drehmoment errechnet sich demnach durch 


Tra=D(g)v+g(g), (5.2) 


wobei mit v der neue Stelleingang bezeichnet wird. Der Stelleingang v ermöglicht nun die 
Formulierung des Trajektorienfolgereglers in der Form 


v=äs-Kı(a-4u)-Kola-a)-Kı [a- dt (5.3) 
mit der gewünschten Trajektorie ga, ga und da (t) = u(t) aus (4.63a), und den positiv 


definiten Diagonalmatrizen Ko, Kı und K,. Es kann einfach gezeigt werden, dass der 
geschlossene Kreis asymptotisch stabil ist. 


5.2 Greifhand 


Im folgenden Abschnitt wird ein Trajektorienfolgeregler für die Fingergelenke der Greifhand 
entworfen. An dieser Stelle sei erwähnt, dass es durch die interne Steuerung der Hand 
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nicht möglich ist, direkt die Drehmomente der Fingergelenke vorzugeben. In der internen 
Steuerung der Greifhand wird einer der folgenden Regler festgelegt: 


1. Ein Positionsregler für q) mit konstanter maximaler Geschwindigkeit |Qn| = 4n,maz 
und konstanter maximaler Beschleunigung |än| = än,maz- 


2. Ein Geschwindigkeitsregler für Q, mit einer konstanten internen Beschleunigung 
lär| = Ön,maz: 


3. Ein Geschwindigkeitsregler für Q„n mit vorgegebener Beschleunigung |än|. 


Nähere Angaben zur Konfiguration der Hand folgen in Kapitel 6. 

Als interner Regler wird der Geschwindigkeitsregler für q„ mit einer konstanten internen 
Beschleunigung |än| = än,maz gewählt. Da der interne Regler nur auf die gewünsch- 
te Geschwindigkeit 4n,a regelt, können Abweichungen zwischen den gewünschten qn,a 
und den tatsächlichen Zuständen q, auftreten. Um dem entgegen zu wirken, wird eine 
Regelkreisstruktur wie in Abbildung 5.1 aufgebaut. Der Sollwert für den internen Ge- 
schwindigkeitsregler 4n,s bildet sich aus der Summe des Steuerungsanteils qn,a und des 
Regleranteils gn,. eines PI-Reglers, entsprechend 


Gn,s = Gn,da + An,e- (5.4) 
Für den PI-Regler gilt 
An, = Kran,e+ Kı / Qn,edt, Gn,e = Qn,d — 4h (5.5) 


mit den Diagonalmatrizen Kp und K;. Die gewählten Werte für Kp und K; sind der 
Tabelle A.7 zu entnehmen. 


Vorsteuerung Greifhand 


PI-Regler 


Abbildung 5.1: Blockschaltbild der Regelkreisstruktur der Greifhand. 
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6 Implementierung und Ergebnisse 


Der erste Teil dieses Kapitels beinhaltet eine genauere Beschreibung des Versuchsaufbaus 
und wie die einzelnen Komponenten miteinander kommunizieren. Im zweiten Teil ist 
eine offline geplante Trajektorie für eine zufällige Objektpose dargestellt. Zusätzlich 
wird für vier zufällige Objektposen ein Greifen ausgeführt. In Abschnitt 6.3 wird eine 
Monte-Carlo-Simulation der Offiine-Trajektorienplanung durchgeführt und die Erfolgsrate 
beurteilt. 


6.1 Implementierung 


Dieses Kapitel beschreibt die Implementierung des Versuchs und geht auf die erzielten 
Ergebnisse ein. Der Versuchsaufbau ist in Abbildung 6.1 dargestellt. Der KUKA Roboter 
ist über EtherCAT mit dem PC verbunden. Die Regelung des Roboters erfolgt über 
das vom ACIN zur Verfügung gestellte MATLAB/SIMULINK Modell. Durch das Beckhoff 
TE1400 TC3 Target for Simulink® wird aus dem Modell ein Modul generiert, das im 
TwinCAT geladen wird und in der Echtzeitumgebung von TwinCAT mit einer Zykluszeit 
von 125 ys läuft. Das zur Verfügung gestellte MATLAB/SIMULINK Modell wurde um das 
Kalman-Filter aus Abschnitt 3.2 und die Berechnung der Trajektorie (4.63a) erweitert. Der 
Trajektorienfolgeregler aus Abschnitt 5.1 ist bereits Teil des ACIN-Modells. Des Weiteren 
ist unterlagert noch eine Regelung basierend auf der singulären Störtheorie sowie eine 
Reibungskompensation. Die Laufzeit t, mit welcher die Solltrajektorie berechnet wird, 
wird manuell vom Zeitpunkt t = Os gestartet. 

Die Schunk Greifhand ist über eine weitere Netzwerkkarte mit dem PC verbunden. Für 
die Steuerung und Regelung nach Abschnitt 5.2 wurde ein weiteres MATLAB/SIMULINK 
Modell erstellt, welches unabhängig vom Roboter in einer eigenen TwinCAT RT Um- 
gebung mit einer Zykluszeit von 20 ms läuft. Es wurde ein TwinCAT C/C++ Modul 
programmiert, welches für den zyklischen Datenaustausch der internen Steuerung der 
Greifhand und dem MATLAB/SIMULINK Modell über Real-Time-Ethernet zuständig ist. 
Im MATLAB/SIMULINK Modell kann manuell einer der zur Verfügung stehenden inter- 
nen Regler der Greifhand (siehe Kapitel 5.2) vorgewählt werden. Zur Auswahl steht ein 
Positionsregler mit konstanter maximaler Geschwindigkeit |Qn| = 4n.maz und konstanter 
maximalen Beschleunigung |Gn| = är,max: Des Weiteren können zwei unterschiedliche 
Geschwindigkeitsregler g), mit der maximalen Beschleunigung |Qn| = An,maz bzw. mit 
vorgegebener Beschleunigung |d7)| der Fingergelenke, ausgewählt werden. Je nach ausge- 
wähltem Regler werden unterschiedliche Steuerkommandos an die Greifhand gesendet. 
Das erstellte C/C++ Modul sendet je nach Reglerauswahl die erforderlichen Befehle, liest 
die internen Zustände der Hand und leitet sie an das MATLAB/SIMULINK Modell weiter. 
Zusätzlich beinhaltet das MATLAB/SIMULINK Modell die Berechnung der Solltrajektorie 
(4.63b) und den Trajektorienfolgeregler aus Abschnitt 5.2. Die Laufzeit t des Versuchs 
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wird aus dem MATLAB/SIMULINK Modell des Roboters ausgelesen. Aus Gründen der Per- 
formance können die Steuerbefehle maximal mit 50 Hz an die Greifhand gesendet werden. 
Die Greifhand ist mit drucksensitiven Flächen ausgestattet, die in dieser Arbeit jedoch 
nicht ausgewertet wurden. Genauere Informationen über die möglichen Einstellungen sind 
dem Manual zu entnehmen [22]. 

Die Kamera ist über Gigabit mit dem PC verbunden. Für das Auslesen der Bilder 
wird das Python Modul pypylon verwendet [23], welches von Basler zur Verfügung 
gestellt wird. Die Bilder werden in einem Python Skript zyklisch ausgelesen. Für die 
Bildverarbeitung, das Erkennen der ArUco-Marker, das Extrahieren der Punktematrix B 
und das Berechnen von (3.6) werden die jeweiligen Implementierungen der frei verfügbaren 
Bibliothek OpenCV Version 4.2 inklusive der Extramodule verwendet [24, 25]. In jedem 
Bild werden die ArUco-Marker detektiert, die Pose des Koordinatensystems (0.%0Yo2o) 
in Bezug zur Kamera (0.x.ye2ze) berechnet und über die ADS-Bibliothek [26] an das 
Kalman-Filter im Roboter-Modell weitergeleitet. 

Das Online-Update aller Teiltrajektorien erfolgt über ein weiteres Python Skript. In 
dem Skript werden aus dem Robotermodell über ADS die aktuelle Zeit tacı = t und 
die geschätzte Pose des Objekts in Bezug zur Kamera ausgelesen. Durch das Kalman- 
Filter wird die Roboterbewegung zwischen zwei Bildern berücksichtigt und somit ist eine 
genauere Posenschätzung möglich. Zusätzlich wird durch das Kalman-Filter der Einfluss 
von Ausreißer erheblich reduziert und es kann auf eine Detektion und Entfernung von 
Ausreißern verzichtet werden. Aus der geschätzten Pose und dem Robotermodell wird 
die Pose des Objekts in Bezug zum Basiskoordinatensystems berechnet und anschließend 
wie in Kapitel 4.5 beschrieben ein Update der ersten Teiltrajektorie durchgeführt. Durch 
das Update ergeben sich neue Kontrollpunktvektoren P;,i €e f{=0,1,...,N +2} für 
(4.62). Diese Kontrollpunktvektoren werden über ADS an die Trajektorienberechnung 
im Robotermodell übergeben. Der Zeitoffset At’ in (4.69) wird so groß gewählt, dass 
das Trajektorienupdate innerhalb At’ abgeschlossen ist und somit mögliche Sprünge in 
der Solltrajektorie vermieden werden, auch wenn es sich nur um kleine Sprünge handelt. 
Dieses Update der ersten Teiltrajektorie wird im Zeitintervall t € [0;0,9T,] in einer 
Schleife ausgeführt. Gilt für die aktuelle Zeit tacı > 0,97,, wird einmalig ein Update der 
zweiten und der dritten Teiltrajektorie nach Abschnitt 4.5 durchgeführt und die neuen 
Kontrollpunktvektoren der Trajektorie an das Robotermodell übergeben. 

Die Optimierungsprobleme aus Kapitel 4 wurden alle mit dem Interior Point Optimizer 
[27], kurz IPOPT, und dem Linear Solver MA57 [28, 29] gelöst. Die Ableitungen der 
Kostenfunktionen und der Beschränkungen wurden über das Open-Source Tool CasADi 
[30] realisiert, welches automatic differentiation (AD) ermöglicht. 


6.2 Ergebnisse 


Im folgendem Abschnitt werden einige Ergebnisse dargestellt und diskutiert. Ursprünglich 
war eine Objekterkennung mittels Künstlicher Intelligenz vorgesehen. Dazu wurden die 
Neuronale Netze aus der Singleshotpose-Methode [5], kurz SSP, und der PVnet-Methode 
[4] implementiert. Für das Training der Netzwerke wurde unter Zuhilfenahme des NVIDIA 
Deep learning Dataset Synthesizer [6] ein Datenset aus 10000 Bildern erstellt. Für eine 
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Abbildung 6.1: Übersicht des Versuchsaufbaus. 
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Bildauflösung von 1024 x 1024 Px hat das Netzwerk aus der PVnet-Methode eine Inferenz- 
zeit von rund 200 ms und ist somit für eine Real-Time Anwendung weniger geeignet. Im 
Gegensatz dazu liegt die Inferenzzeit des Netzwerks der SSD-Methode bei rund 40 ms. Es 
hat sich bei beiden Varianten gezeigt, dass die Genauigkeit der geschätzten Pose stark von 
der Objektgröße im Bild abhängig ist. Mit zunehmender Größe steigt der Fehler zwischen 
der geschätzten und der tatsächlichen Pose. Ist die Entfernung zwischen Kamera und 
Objekt zu klein, scheitern beide Varianten. Aus diesem Grund wurde zu der in Kapitel 3 
beschriebenen Methode mit ArUco-Markern gewechselt. 

Zu Beginn des Versuchs befindet sich der Roboter in einen der beiden gewählten 
Initialzustände 


go = [0°,0°,0°,—40°,0°,100°,90°], (6.1a) 
= = [55°,39,5°,—6,6°,—22°,—36,3°,110,6°,134°]. (6.1b) 


Als Zielpunkt bzw. Endzustand wird 
qT = [0°,0°,0°,—40°,0°,100°,90°] (6.2) 


festgelegt. Das Objekt wird im Sichtfeld der Kamera am Boden platziert und anhand der 
Schätzung der Pose offline eine Trajektorie geplant. In den Abbildungen 6.2, 6.3 und 6.4 
ist die über (4.63a) berechnete Trajektorie des Roboters mit dem Initialzustand (6.1b) im 
Zeitintervall te [0,7, + 7a + 73] mit 7, = T3 = 4s und 75 = 0,5 dargestellt. Der blaue 
Teil der Trajektorie ist die Roboterbewegung vom Initialzustand des Roboters bis zum 
Beginn des Greifens, der rote Teil zeigt die Roboterbewegung während des Schließens 
der Finger, und der grüne Teil zeigt die Bewegung zur Zielposition nach dem Greifen. 
Wie gewünscht gehen die einzelnen Teiltrajektorien stetig ineinander über. Die über 
(4.63b) berechnete Trajektorie der Fingergelenke im Zeitintervall t € [T1,Tı + Th] zeigt 
Abbildung 6.5. Finger 1 und 3 sind bis zum Zeitpunkt t = T| symmetrisch zueinander, 
somit gilt gne(t <= Tı) = elt <= Tı) sowie grs(t <= Tı) = Mr(t <= Tı). Die 
geplanten Eingangsgrößen uni, € {2,3,6,7} sind ebenfalls dargestellt, werden jedoch für 
die Regelung der Fingergelenke nicht benötigt. Sie wurden mitgeführt, um denselben 
Code für die Interpolation und dieselbe Struktur für die Trajektorienplanung verwenden 
zu können. Die Darstellung zeigt, dass mit der festgelegten Zeitdauer von Ta = 0,55 
fast die kürzeste realisierbare Zeit für das Greifen erreicht ist, da der Gelenkswinkel gn« 
mit konstanter maximaler Beschleunigung beschleunigt und wieder abbremst. Die aus 
der Trajektorie der Gelenkswinkel q, berechnete Trajektorie der Greifzustände ga ist in 
Abbildung 6.6 dargestellt. Sie zeigt, dass der im Optimierungsproblem (4.19) geforderte 
lineare Anstieg von yc und somit die Gleichungsbeschränkung (4.19c) eingehalten wird. 
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Abbildung 6.2: Interpolierte Teiltrajektorien der Gelenkswinkel qı,... 
aus (4.44) in blau, (4.30) in rot und (4.46) in grün. 
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Abbildung 6.3: Interpolierte Teiltrajektorien der Gelenkswinkelgeschwindigkeiten q1,... 


des Roboters aus (4.44) in blau, (4.30) in rot und (4.46) in grün. 
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Abbildung 6.4: Interpolierte Teiltrajektorien der Gelenkswinkelbeschleunigungen äi,... 


des Roboters aus (4.44) in blau, (4.30) in rot und (4.46) in grün. 
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Abbildung 6.5: Interpolierte Teiltrajektorien der Fingergelenke aus (4.19). 
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Abbildung 6.6: Greifzustände aus den interpolierten Teiltrajektorien der Fingergelenke 
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Mit jedem Initialzustand in (6.1) wurden zwei Versuche mit einer zufälligen Objektpose 
durchgeführt, dargestellt in den Abbildungen 6.7-6.12. Die Trajektorien der Gelenksgrößen 
q sind in Abbildung 6.7 mit dem Initialzustand (6.1a) sowie in Abbildung 6.10 mit dem 
Initialzustand (6.1b) dargestellt. Die Trajektorien der Geschwindigkeiten q sind in den 
Abbildungen 6.8 und 6.11 abgebildet, sowie die Trajektorien der Beschleunigungen in den 
Abbildungen 6.9 und 6.12. Die Abbildungen enthalten die geplanten Trajektorien aus der 
Ofline-Trajektorienplanung in blau und orange und die upgedateten Trajektorien aus 
dem Online-Trajektorienupdate am Ende des Versuchs in rot und grün. Wie zu sehen ist, 
sind auch nach mehreren online Trajektorienupdates die Trajektorien stetig und es treten 
keine Sprünge auf. Es zeigt auch, dass der Roboter im Zeitbereich t € [0,7}] mit T, = 4s 
hauptsächlich der ursprünglich geplanten optimalen Trajektorie folgt und erst im letzten 
Viertel t € [3s,4s] größere Abweichungen auftreten. 

In Abbildung 6.13 sind in blau jene Zeitpunkte eingezeichnet, an denen ein Messwert 
für das Kalman-Filter Messupdate zur Verfügung steht, sowie in rot die Zeitpunkte an 
denen ein online Update der Trajektorie ausgeführt wurde. Die Zeitdifferenz zwischen 
den einzelnen Zeitpunkten ist nicht konstant, da die Ausführungszeit für das Auswerten 
der Bilder und Berechnen der Pose nicht konstant ist und nicht in der Echtzeitumgebung 
von TwinCAT implementiert ist. Gleiches gilt auch für das online Update der Trajektorie. 
Abbildung 6.13 zeigt, dass während der Bewegung vom Initialzustand zum Objekt im 
Schnitt 30 Messupdates des Kalman-Filters und 17 Trajektorienupdates stattfinden. Die 
mittlere Zykluszeit für das Messupdate des Kalman-Filters ist 161,3 ms und 185 ms für 
das Trajektorienupdate. 

Während der Versuche hat sich gezeigt, dass die numerische Berechnung der inversen 
Kinematik nicht immer konvergiert. In diesem Fall wird die Zielposition aus dem vorigen 
Trajektorienupdate übernommen. 
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Abbildung 6.7: Interpolierte Teiltrajektorien der Gelenkswinkel qı,... 


mit Initialzustand (6.1a). 
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Abbildung 6.8: Interpolierte Teiltrajektorien der Gelenkswinkelgeschwindigkeiten gı,... ‚g7 
des Roboters mit Initialzustand (6.1a). 
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Abbildung 6.9: Interpolierte Teiltrajektorien der Gelenkswinkelbeschleunigungen ä1,..- 


des Roboters mit Initialzustand (6.1a). 
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Abbildung 6.10: Interpolierte Teiltrajektorien der Gelenkswinkel qı,... 


mit Initialzustand (6.1b). 
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Abbildung 6.11: Interpolierte Teiltrajektorien der Gelenkswinkelgeschwindigkeiten 
d1,... ‚gr des Roboters mit Initialzustand (6.1b). 
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Abbildung 6.12: Interpolierte Teiltrajektorien der Gelenkswinkelbeschleunigungen 
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‚jr des Roboters mit Initialzustand (6.1b). 
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(a) Erster Versuch mit Initialzustand (6.1a). 
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(b) Zweiter Versuch mit Initialzustand (6.1a). 
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(c) Erster Versuch mit Initialzustand (6.1b). 
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(d) Zweiter Versuch mit Initialzustand (6.1b). 


Abbildung 6.13: Zeitpunkte der Trajektorienupdates in rot und der Messupdates des 
Kalman-Filters in blau während der Roboterbewegung von der Initialpo- 


sition zum Objekt. 
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6.3 Offline- Trajektorienplanung Monte-Carlo-Simulation 


In diesem Abschnitt wird eine Monte-Carlo-Simulation der Offline- Trajektorienplanung 
durchgeführt. Für eine statistische Auswertung der Erfolgsrate und um mögliche Be- 
reiche mit einer niedrigen Erfolgsrate zu finden, wurde für 20000 zufällige gleichverteil- 
te Objekt-Posen eine OMliine-Trajektorienplanung durchgeführt. Der Roboter befindet 
sich in der Initialposition (6.1a). Die Position des Objekts ist dabei eine gleichverteil- 
te Zufallsvariable Pd = [P8.25 98,4; 8.2] € Ip&P3) mit p) = [0,6 m; —0,2m; 0,15 m] und 
ps = [0,75 m; 0,2 m; 0,25 m]. Die Orientierung des Objekts ist mit den gleichverteilten 
Zufallsvariablen der Roll-, Nick- und Gierwinkel a € [-2,5°; 2,5°), # = |-15°; 15°] und 
y = [-2,5°; 2,5°] über , 

R) = R, sRy Rz, aRy.sRz.o, (6.3) 


festgelegt. Mit der zufälligen Objekt-Pose [Rg ‚B3] wird eine Ofline-Trajektorienplanung 
nach Abschnitt 4.1 bis 4.3 durchgeführt und anschließend das Status-Bit des Optimie- 
rungsalgorithmus für jedes einzelne Optimierungsproblem ausgewertet. Das Status-Bit 
gibt an, ob eine optimale Lösung gefunden wurde. Die Anzahl der gefundenen optimalen 
Lösungen sowie die Erfolgsraten sind in Tabelle 6.1 angegeben. Die Auswertung zeigt, 
dass die Berechnung der Robotertrajektorie während des Schließens der Finger (4.30) und 
der Roboterbewegung zur Zielposition nach dem Greifen (4.46) die geringste Erfolgsrate 
haben. Alle anderen Optimierungsprobleme haben eine Erfolgsrate von über 90%. An 
dieser Stelle sei erwähnt, dass jedes gelieferte Ergebnis des Optimierungsalgorithmus aus 
(4.30) verwendet wird, um eine optimale Lösung für (4.44) und (4.46) zu finden. Eine 
Verbesserung der Erfolgsrate von (4.30) könnte somit direkt in eine Verbesserung der 
Erfolgsrate von (4.44) und (4.46) resultieren. 


Optimierungsproblem | (4.7) (4.17) (4.19) (4.30) (4.44) (4.46) 


Erfolgreich 20000 19598 20000 13440 18780 15845 
Erfolgsrate 100% 98% 100% 67,2% 93,9% 79,2% 


Tabelle 6.1: Erfolgsrate der einzelnen ÖOptimierungsprobleme der Offline- 
Trajektorienplanung mit 20000 zufälligen Objekt-Posen. 


Um Bereiche der Objektposen [R3.B0] zu finden, in denen leicht oder kaum eine optimale 
Trajektorie berechnet werden kann, sind die ersten 10000 Simulationsergebnisse in der sog. 
Streumatrix in Abbildung 6.14 dargestellt. Dabei wird die Farbe rot verwendet, wenn für 
jedes einzelne Optimierungsproblem eine optimale Lösung gefunden werden konnte. Liefert 
mindestens ein Optimierungsproblem nicht die optimale Lösung, ist der jeweilige Punkt 
in blau eingezeichnet. Auf der Diagonalen der Streumatrix sind die Histogramme der 
Zufallsvariablen dargestellt. Mit ihnen kann der Einfluss der jeweiligen Zufallsvariablen 
auf das Lösungsverhalten beurteilt werden. Für die Winkel 3 und 7 zeigt sich z.B. ein 
besseres Konvergenzverhalten für positive Werte. Während das Konvergenzverhalten 
für ß linear steigt, ist für y eine sprungförmige Verbesserung um den Nullpunkt zu 
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beobachten. Mit zunehmender Entfernung des Objekts in Bezug zur Roboterbasis p9 .„ 
zeigt sich eine leichte Verschlechterung. Das Histogramm der anderen Zufallsvariablen ist 
annähernd konstant und somit für den simulierten Bereich kaum eine Abhängigkeit auf 
das Lösungsverhalten erkennbar. Aus den einzelnen Plots in der linken unteren Hälfte 
lassen sich ebenfalls die Bereiche für eine gute bzw. schlechte Erfolgsrate identifizieren. 
Jene Bereiche mit einer größeren Dichte an blauen Punkten stellen die Bereiche mit einem 
schlechten Lösungsverhalten dar. Die rechte obere Hälfte wird nicht weiter kommentiert, 
da sie die gespiegelte Version der linken unteren Hälfte darstellt. 


—— nicht optimal; —— optimal 


en a & 
-100 10 2 0.75—0.2 0 0.2 0.15 0.2 0.25 


: . : Per j a 
ain Bin yin Po, nm po, nm po, nm 


Abbildung 6.14: Streumatrix der Monte-Carlo-Simulation der Offline-Trajektorienplanung. 
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7 Zusammenfassung 


In dieser Arbeit wurde erfolgreich eine Methode für das Greifen eines optisch detektierten 
Objekts entworfen. Dazu wurde auf einem Industrieroboter eine Greifhand und eine Kamera 
montiert. Durch eine Objekterkennung wurde das Objekt in den Bildern detektiert und 
die Lage geschätzt. Für die geschätzte Lage des Objekts wurde offline eine Trajektorie 
für den Roboter und die Finger der Greifhand geplant. Ziel war es, dass sich der Roboter 
während des Schließens der Finger kontinuierlich weiterbewegt. Die verwendete Methode 
der Objekterkennung ist mit Schätzfehlern behaftet. Je geringer die Entfernung zwischen 
dem Objekt und der Kamera, umso geringer sind die Schätzfehler. Um auf die besser 
werdenden Schätzungen der Pose während der Greifbewegung zu reagieren, wurde ein 
schnelles online Update der Robotertrajektorie implementiert. 

Im ersten Teil dieser Arbeit wurden die mathematischen Modelle hergeleitet. Da die 
exakten Masseverteilungen der Greifhand und der Kamera nicht bekannt sind, wurden sie 
als Punktmasse modelliert und die Parameter geschätzt. Der verwendete Roboter wurde mit 
den zwei Punktmassen als Endeffektor erweitert und über den Euler-Lagrange Formalismus 
die Bewegungsgleichungen in der üblichen Form hergeleitet. Für einen erfolgreichen Griff 
wurden neue Greifzustände eingeführt und eine Vorwärts- bzw. Rückwärtskinematik zum 
Umrechnen zwischen den Fingerzuständen und den Greifzuständen hergeleitet. Aufgrund 
der internen Steuerung der Greifhand wurde auf ein dynamisches Modell verzichtet. 

Für die Objekterkennung wurden am Objekt sog. ArUco-Marker angebracht. Die Marker 
können im Bild schnell identifiziert werden. Aus den bekannten Abständen zwischen 
den Eckpunkten und den Positionen am Objekt wurde die Lage des Objekts in Bezug 
zur Kamera berechnet. Um den Einfluss etwaiger Ausreißer zu verringern und eine 
bessere Schätzung zwischen den Kamerabildern zu ermöglichen, wurde ein Kalman-Filter 
entworfen. Die Eingangsgrößen des Kalman-Filters sind die lineare Geschwindigkeit und die 
Winkelgeschwindigkeit der Kamera. Mit der geschätzten Pose anhand der ArUco-Marker 
wurde ein Messupdate des Kalman-Filters durchgeführt. 

Durch eine Offline-Trajektorienplanung wurde anhand der geschätzten Pose des Kalman- 
Filters die Bewegung des Roboters und der Greifhand geplant. Dazu wurde zunächst am 
Objekt ein Greifpunkt definiert. Die Trajektorie der Finger wurde so geplant, dass sich 
der Greifpunkt zwischen den Fingern und der des Objekts überlagern und die Oberfläche 
der Finger tangential zur Objektoberfläche ist. Die Trajektorie des Roboters wurde in drei 
einzelne Trajektorien unterteilt. In der ersten Teiltrajektorie bewegt sich der Endeffektor 
von der Initialposition zum Objekt, in der zweiten Teiltrajektorie schließen sich die 
Finger und in der dritten Teiltrajektorie befindet sich das Objekt an der Greifhand 
und der Endeffektor bewegt sich zur Zielposition. Für die erste Teiltrajektorie wurden 
Beschränkungen definiert, damit das Objekt immer im Sichtfeld der Kamera bleibt. 

Die geplanten Trajektorien des Roboters und der Greifhand wurden vorgesteuert. Um 
etwaige Abweichungen gering zu halten bzw. zu eliminieren, wurde jeweils ein Trajek- 
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torienfolgeregler entworfen. Da die Zentrifugal- und Corioliskräfte bei den auftretenden 
Geschwindigkeiten nur einen geringen Einfluss haben, wurden sie vernachlässigt. 

Die entwickelte Methode wurde in mehreren Versuchen verifiziert und getestet. Während 
der ersten Teiltrajektorie werden im Schnitt 17 erfolgreiche Updates durchgeführt. Dabei 
hat sich gezeigt, dass die numerische Berechnung der neuen Zielposition über die inverse 
Kinematik nicht immer konvergiert. In diesem Fall wurde die Zielposition aus dem vorigen 
Update übernommen. 
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A Anhang 


A.1l Parameter 


A.1.1 Translationen und Rotationen zur Beschreibung der Kinematik 


In Tabelle A.1 sind die Rotationsmatrizen und Translationsvektoren zur Beschreibung der 
Kinematik des Gesamtsystems aus Abschnitt 2 angegeben. 


\ T j 
KOSi KOS;j Translation (d} ) Rotationsmatrix R} (q) 


oO 


POoOnoutPkroavmxmrbte INN OT PRWMN Hm 


zoo nıo wur wamN Hm 


[0,0,l0] 
[0, - di lı] 
[0, - Ia,dı] 

[0,da ‚l3] 

[0,l4,ds] 
[0, zu ds ls] 
[0, — Ig,ds] 

[0,0,77] 

[0,de,l7 Ar I] 
[0,0,17 + In] 
Sl112,S11y»Sllz 
5122 ,S12y,S12z 
EIERZETEITEEIRF 
S14x ,S14y,S14z 
San, S15yrS15z 
51625169 3162 
5172,517y»317z 
Be] 
[Enz 35; 


Tabelle A.1: Translation und Rotation zwischen den Koordinatensystemen (0;x;y;2;) und 
(0,2jYyj2;) für die Beschreibung der Kinematik des Gesamtsystems. 


A.1.2 Roboter KUKA LBR iiwa R14 820 


Die Parameter des Roboters aus Abbildung 2.1 sind in Tabelle A.2 angegeben. 
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A Anhang A Anhang 76 
Parameter Value Parameter Value 
lo 1525-10 m Tier 69,076 - 10”? kgm? 
lı 2075-10 m Teig 71,108:-10°° kgm? 
la 2325-10”? m Ts 20,062-10-° kgm? 
la 1875-10 m Tnosse 82,470:-10°° kgm? 
la 2125-10 m EISEN 16,411-10°? kgm? 
l; 1875-107 m Tod: 87,351-10°° kgm? 
Is 79,6-10°° m Tas 23,015-10-? kem? 
Ir 724-10° m Tea 22,866 10°? kgm? 
1; 69,04-10°° m Is. 55,448-10=° kgm? 
In 1043-10 m TpA. 24 47,278-10°° kgm? 
dı 13:10% m Id) 9,647:10°°  kgm? 
da 11:10 m Titan 46,652-10°?° kgm? 
ds 62-10 m Tas. 13,835 10° kgm? 
de 135:10% m Isa 11,685. 10°? kgm? 
mp 6,495 kg Dass 5,718-10°?° kgm? 
mia 8,807 kg Ts 7327-107 kem? 
mp3 2,8 kg Iso 4,776-10° kgm? 
mp 5,283 kg Ten 6,067:10°° kgm? 
mas 1,889 kg Tpr.z3 0,884:10°?° kgm? 
Mi6 2,32 kg Tpray 0,888-10-° kgm? 
mp 0,863 kg Iyrss 1,105-10°° kgm? 
Me 0,63 kg Tyan 1,662-10°? kgm? 
mn 2,49 kg Tag 0,823-10-° kgm? 
Tyass 1,317:10° kam?’ 
Ds 8,200 :10°?” kgm? 
Thy 8,254:10°% kgm? 
IR 3,503-10°° kgm? 


Tabelle A.2: Parameter des KUKA LBR iiwa 14 R820 Roboters. 
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A Anhang A Anhang 77 


A.1.3 Greifhand Schunk SDH2 


Die Parameter der Schunk Greifhand SDH2 aus Abbildung 2.3 sind in Tabelle A.3 
angegeben. 


Parameter Wert 

hı 415:10° m 
bı 33-10 m 
lı 19,05-10°° m 
ha 86,5-10°? m 
bu 15,29-10°° m 
brı 14,82-10° m 
bra 515-10°° m 
hrı 18,51:10°° m 
hra 33,26-10°? m 
hr3 475-110 m 
hra 66,06-10°° m 
rrı 60:10” m 
rT2 61:10 m 
ST 66,26:10°°” m 
sc 56,75:10°° m 
d 54,42 ® 
n 4,46 . 
dh1,0 90 ° 
42,0 —10 ° 
4n3,0 0 ; 
Qn4,0 90 : 
In5,0 60 a 
Qn6,0 19 ö 
Ih7,o 0 i 


Tabelle A.3: Parameter der Greifhand Schunk SDH2. 


A.1.4 Kalibrierung der Kamera 


Eine Kalibrierung der Kamera mit Hilfe eines Schachbrettmusters liefert die in Tabelle 
A.4 angegebenen intrinsischen Parameter der Kamera. 


A.1.5 Kalman-Filter 


Die verwendeten Kovarianzmatrizen des Kalman-Filters sind in Tabelle A.5 angegeben. 
Die Werte für R, sind aus den Rauscheigenschaften der Posenschätzung des Objekts 
abgeleitet. Um die Auswirkung von Ausreißern gering zu halten, wurden die Werte für Q;. 
manuell gewählt. 
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A Anhang A Anhang 78 


Parameter Wert 


fi 1122,98 
fu 1122,28 
Cr 513,52 
Cy 507,50 


Tabelle A.4: Intrinsischen Parameter der Kamera Basler avA1000-100gc. 


Parameter Wert 
R, diag ([0,0004; 0,0004; 0,0004; 0,0004; 0,0004; 0,0004; 0,0004] 
Q: diag ([2,5; 2,5; 2,5; 10; 10; 10]) 


Tabelle A.5: Kovarianzmatrizen des Kalman-Filters. 


A.1.6 Reglerparameter 


In diesem Abschnitt sind die Parameter der Trajektorienfolgeregler des Roboters in Tabelle 
A.6 und der Greifhand in Tabelle A.7 angegeben. 


Parameter Wert 

Ko diag ([1200; 1200; 1200; 1200; 1200; 1200; 1200]) 
Kı diag ([60; 60; 60; 60; 60; 60; 60]) 

K; diag ([8000; 8000; 8000; 8000; 8000; 8000; 8000] ) 
K- diag ([4; 4; 4; 5; 3; 2,5; 2,5]) 

D- diag ([0,015: 0,015; 0,015; 0,02; 0,01: 0,01; 0,01]) 
B diag ([10,83; 10,83; 4,89; 4,89; 1,47; 0,47; 0,47]) 


Tabelle A.6: Parameter des Trajektorienfolgereglers des Roboters. 


A.1.7 Beschränkungen 
Die gültigen Beschränkungen aller Größen sind in Tabelle A.8 angegeben. 
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Par. 


Parameter Wert 
Kp diag ([2; 2; 2; 2; 2;2;2]) 


Tabelle A.7: Parameter des Reglers der Greifhand. 


Wert 


[Ken Bean Mes lo Be De = 22 ES 


Q 
[D} 


[O mm; —10°; —30 mm; 160 mm] 
[80 mm; 10%; 30 mm; 170 mm] 
[0°; 90°; —90°; —90°; —90°; —90°; —90°] 
[90°; 90°; 90°; 90°; 90°; 90°; 90°] 

[-80 °/s; —100°/s; —100°/s; —100°/s; —100 °/s; —100°/s; —100°/s] 
[80 °/s; 100 °/s; 100 °/s; 100 °/s; 100 °/s; 100 °/s; 100 °/s] 
1000-[-5 °/s?; —0,4° /s2; —1,5 °/s?; —0,4°/s?; —1,5 °/s?; —0,4°/s?; —1,5° /s2] 
100058: 01 tler ee ee 
[-170°; —120°; —170°; —120°; —170°; —120°; —175°] 

[170°; 120°; 170°; 120°; 170°; 120°; 175°] 

0,4-[-85 ° /s; —85 °/s; —100 °/s; -75 °/s; —130°/s; —135 °/s; —135 °/s] 
0,4185 * /s; 85 °/s; 100° /s; 75° /8; 130 °/s: 135° /s; 135 °/8] 
0,4-[320 N m; 320 Nm; 176N m; 176 Nm; 110 Nm; 40 Nm; 40 Nm] 
0,4-[320 N m; 320 Nm; 176 N m; 176 Nm; 110 Nm; 40 Nm; 40 Nm] 
[-8,5 Nm; -10 Nm; -5N m; —5,7Nm; —1,5 Nm; —1,8Nm; —0,25 N m] 
[8,5 Nm; 10Nm; 5N m; 5,7Nm; 1,5 Nm; 1,8Nm; 0,25 N m] 

As 
75 
4s 
75 
[O px; Opx] 

[1024 px; 1024 px] 

[-50 px; —50 px] 

[50 px; 50 px] 

[-0,5 mm; —0,5 mm; —0,5 mm] 


[0,5 mm; 0,5 mm; 0,5 mm] 
[-0,5 mm; —0,5 mm; —0,5 mm] 


[0,5 mm; 0,5 mm; 0,5 mm] 


Tabelle A.8: Beschränkungen 
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